diff --git a/.agents/skills/create-module/SKILL.md b/.agents/skills/create-module/SKILL.md index ae3d4901e..72e121d50 100644 --- a/.agents/skills/create-module/SKILL.md +++ b/.agents/skills/create-module/SKILL.md @@ -101,6 +101,27 @@ is remapping other modules' topics or overriding canonical names in module launc files: in a conventional stack, including the module must require **zero** remaps, so only deviations appear in stack files.) +## Heavy dependencies: overlay layer vs sidecar container + +Two patterns exist for deps that do not belong in the trunk robot image: + +- **Overlay layer** (`dockerfile: Dockerfile.module`, tier 2) — the module's + deps are built ON TOP of the robot image and its nodes run in the robot + container (asm_macvo: TensorRT + weights). Right when the module's nodes must + share the robot's process space/ROS distro and the deps are compatible with + the robot image's Ubuntu/Python. +- **Sidecar container** (`compose: .yaml` with its own `build:`) — + a second container next to the robot on `airstack_network`, same + `ROS_DOMAIN_ID`, talking DDS (asm_raven: RayFronts on Ubuntu 22.04 / ROS 2 + Humble / torch cu130 / patched OpenVDB — incompatible with the Jazzy robot + image). The overlay absolutizes the fragment's `build.context`, + `build.dockerfile`, `env_file` and bind sources; `airstack up` builds the + image on first run. Keep a thin robot-side colcon package (`_bridge`) + that owns the canonical launch file and relays the sidecar's module-private + topics onto canonical names — the launch-arg interface must still exist on + the robot side. Cross-distro DDS works for unchanged message types; note it + in the README. + ## Steps (by hand, until `airstack module create` lands) 1. Create the repo with the anatomy above; write `module.yaml` first. diff --git a/.env b/.env index 32ec8c036..070b957f6 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. # auto-generated from git commit hash -VERSION="0.21.0-dev.13" +VERSION="0.21.0-dev.14" # Image-tag discriminator ONLY (appears in the image tag suffix, e.g. ..._robot-x86-64_dev). # No Dockerfile consumes it: "prebuilt" does NOT bake the built ros_ws into the image today — # a real prebuilt (workspace-baked) stage is future work. Keep "dev" (mounted code, built live). diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index cf6fbda9f..866757e7e 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -11,7 +11,6 @@ "/home/andrew/Development/AirStack/robot/ros_ws/src/behavior/behavior_executive/include/**", "/home/andrew/Development/AirStack/robot/ros_ws/src/behavior/behavior_tree/include/**", "/home/andrew/Development/AirStack/robot/ros_ws/src/global/planners/ensemble_planner/include/**", - "/home/andrew/Development/AirStack/robot/ros_ws/src/global/planners/exploration/include/**", "/home/andrew/Development/AirStack/robot/ros_ws/src/global/planners/random_walk/include/**", "/home/andrew/Development/AirStack/robot/ros_ws/src/global/world_models/vdb_mapping/include/**", "/home/andrew/Development/AirStack/robot/ros_ws/src/global/world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/**", diff --git a/docs/development/stacks.md b/docs/development/stacks.md index 8f7e28d74..6295b9e4c 100644 --- a/docs/development/stacks.md +++ b/docs/development/stacks.md @@ -30,6 +30,8 @@ Anatomy is enforced by a unit test: `tests/meta/test_stack_layout_contract.py` | [`full_default`](https://github.com/castacks/AirStack/tree/develop/stacks/full_default) | The full-autonomy topology with the MIGHTY map-based local planner (`asm_mighty` module: planner + acl-mapping voxel world model + NavigateTask bridge) — the baseline, and what launches when no stack is selected. | | [`full_droan`](https://github.com/castacks/AirStack/tree/develop/stacks/full_droan) | The GPU DROAN reactive planner (`droan_gl`, `asm_droan` module) in place of MIGHTY — for depth-camera-only vehicles and legacy comparisons. | | [`full_droan_cpu`](https://github.com/castacks/AirStack/tree/develop/stacks/full_droan_cpu) | CPU DROAN planner + live `disparity_expansion` (`asm_droan` module) for machines without a GPU planner. | +| [`full_exploration`](https://github.com/castacks/AirStack/tree/develop/stacks/full_exploration) | `full_default` with the frontier-based geometric exploration planner in place of the random walk. Requires the `asm_exploration_planner` module (pinned in the stack's `modules.repos` alongside `asm_mighty`). | +| [`full_raven`](https://github.com/castacks/AirStack/tree/develop/stacks/full_raven) | `full_default` with RAVEN open-set semantic navigation as the global planner: RayFronts GPU sidecar container + `raven_bridge`. Requires the `asm_raven` module (pinned alongside `asm_mighty`). | | [`full_macvo`](https://github.com/castacks/AirStack/tree/develop/stacks/full_macvo) | MAC-VO as the DROAN planner's disparity source. Requires the `asm_macvo` and `asm_droan` modules. | | [`lite_default`](https://github.com/castacks/AirStack/tree/develop/stacks/lite_default) | Onboard-lite topology, unsplit: interface, sensors, perception, flat Local layer, behavior; **no global, no logging**. | | [`lite_offload_global`](https://github.com/castacks/AirStack/tree/develop/stacks/lite_offload_global) | A **split stack**: `onboard.launch.xml` (= lite topology) + `offboard.launch.xml` (global layer only) + `bridge.yaml`. | @@ -296,6 +298,8 @@ ignored. Select the topology with `--stack`: | The full-autonomy baseline | Nothing — `full_default` launches by default (or be explicit: `--stack full_default`) | | GPU DROAN reactive planner | `--stack full_droan` (`asm_droan` module) | | CPU DROAN topology | `--stack full_droan_cpu` (`asm_droan` module) | +| Frontier exploration global planner | `--stack full_exploration` (requires the `asm_exploration_planner` module) | +| RAVEN semantic (language-prompted) global planner | `--stack full_raven` (requires the `asm_raven` module; first run builds the sidecar image; Isaac Sim with GUI) | | MAC-VO disparity topology | `--stack full_macvo` (`asm_macvo` + `asm_droan` modules) | | Onboard-lite, unsplit | `--stack lite_default` | | Split onboard/offboard | `--stack lite_offload_global:onboard` / `:offboard` (+ generate the router config from `bridge.yaml`) | diff --git a/docs/modules/exploration_planner.md b/docs/modules/exploration_planner.md new file mode 100644 index 000000000..ba627424f --- /dev/null +++ b/docs/modules/exploration_planner.md @@ -0,0 +1,56 @@ +# exploration_planner + + + +> Frontier-based geometric exploration global planner — OpenVDB occupancy map, frontier clustering, bounded viewpoint sampling and an RRT path to the chosen viewpoint, published as global_plan; developed for the CMU AirLab construction-site project + +| | | +|---|---| +| Repository | [castacks/asm_exploration_planner](https://github.com/castacks/asm_exploration_planner) | +| Type | `ros_package` | +| Maintainer | ajong@andrew.cmu.edu | +| License | BSD-3-Clause-Clear | +| Registered ref | [`v0.1.1`](https://github.com/castacks/asm_exploration_planner/tree/v0.1.1) | +| Declared compat | `>=0.21.0-dev.8 <0.22.0` | +| Registry entry | [modules/exploration_planner.yaml](https://github.com/castacks/airstack-modules-index/blob/main/modules/exploration_planner.yaml) | + +## Install + +From an AirStack checkout ([AirStack Modules guide](../development/modules.md)): + +```bash +airstack module add https://github.com/castacks/asm_exploration_planner --version v0.1.1 +airstack up +``` + +`module add` pins the module in `modules.repos` and syncs it into the +gitignored `modules/` overlay; `airstack up` automatically includes the +generated compose override that mounts it into the containers. + +## Compatibility: declared vs verified + +The range `>=0.21.0-dev.8 <0.22.0` is **DECLARED** by the module author +(copied from the module's `module.yaml`). The **VERIFIED** record — rows +stamped exclusively by CI runs of the reusable +[module-system-tests workflow](../development/module_ci.md) — lives in the +registry's [compat/ matrix](https://github.com/castacks/airstack-modules-index/tree/main/compat) +([compat/exploration_planner.yaml](https://github.com/castacks/airstack-modules-index/blob/main/compat/exploration_planner.yaml) once stamped). +A compatibility claim that isn't CI-verified rots: trust the +matrix, read the declaration as intent. + +## Documentation + +- [Module README on GitHub @ `v0.1.1`](https://github.com/castacks/asm_exploration_planner/blob/v0.1.1/README.md) +- *The module repo was not fetched when this page was generated — the* + *links above go to GitHub at the registered ref (failure isolation:* + *an unreachable module repo never fails the docs deploy).* + +## Registered stacks using this module + +- [full_exploration](../../stacks/full_exploration/README.md) + +## Registry notes + +> Extracted from trunk (robot/ros_ws/src/global/planners/exploration, history preserved) together with the bounded-exploration additions from the construction-site demo branch. Publishes a path on global_plan and serves the behavior/global_plan_toggle service; the MIGHTY bridge (full_exploration) follows the published plan directly, while droan_gl-based stacks add the trunk global_plan_navigate_bridge. Consumed by trunk reference stack full_exploration. diff --git a/docs/modules/index.md b/docs/modules/index.md index 6a598ad4a..408abbaa4 100644 --- a/docs/modules/index.md +++ b/docs/modules/index.md @@ -20,9 +20,11 @@ directory and is never hand-edited. |--------|-------------|------|------------|-----------------|-------| | [dfm2_disturbances](dfm2_disturbances.md) | Isaac Sim disturbance library (fan/vent force fields, strobe lights, lens flare) | `isaac_extension` | ajong@andrew.cmu.edu | `>=0.19.0-alpha.18 <0.20.0` | [repo](https://github.com/castacks/asm_dfm2_disturbances) | | [droan](droan.md) | DROAN disparity-space local obstacle avoidance — the GPU (OpenGL) droan_gl planner and the CPU droan_local_planner with its disparity_expansion / disparity_graph / disparity_graph_cost_map world model, serving AirStack's NavigateTask / trajectory_controller seam from a stereo disparity image | `ros_package` | ajong@andrew.cmu.edu | `>=0.21.0-dev.10 <0.22.0` | [repo](https://github.com/castacks/asm_droan) · [full_droan](../../stacks/full_droan/README.md) · [full_droan_cpu](../../stacks/full_droan_cpu/README.md) · [full_macvo](../../stacks/full_macvo/README.md) | +| [exploration_planner](exploration_planner.md) | Frontier-based geometric exploration global planner — OpenVDB occupancy map, frontier clustering, bounded viewpoint sampling and an RRT path to the chosen viewpoint, published as global_plan; developed for the CMU AirLab construction-site project | `ros_package` | ajong@andrew.cmu.edu | `>=0.21.0-dev.8 <0.22.0` | [repo](https://github.com/castacks/asm_exploration_planner) · [full_exploration](../../stacks/full_exploration/README.md) | | [macvo](macvo.md) | MAC-VO learned stereo visual odometry (ICRA 2025 best paper) — macvo_ros2 wrapper around the MAC-VO network, publishing odometry, a covariance-aware point cloud, and the disparity image the local planner can consume | `ros_package` | ajong@andrew.cmu.edu | `>=0.19.0-alpha.18 <0.20.0` | [repo](https://github.com/castacks/asm_macvo) · [full_macvo](../../stacks/full_macvo/README.md) | -| [mighty](mighty.md) | MIGHTY Hermite-spline local planner (MIT ACL, RA-L 2026) with its acl-mapping voxel world model and a bridge to AirStack's NavigateTask / trajectory_controller seam — AirStack's default local planner (the map-based replacement for DROAN) | `ros_package` | ajong@andrew.cmu.edu | `>=0.21.0-dev.10 <0.22.0` | [repo](https://github.com/castacks/asm_mighty) · [full_default](../../stacks/full_default/README.md) | +| [mighty](mighty.md) | MIGHTY Hermite-spline local planner (MIT ACL, RA-L 2026) with its acl-mapping voxel world model and a bridge to AirStack's NavigateTask / trajectory_controller seam — AirStack's default local planner (the map-based replacement for DROAN) | `ros_package` | ajong@andrew.cmu.edu | `>=0.21.0-dev.10 <0.22.0` | [repo](https://github.com/castacks/asm_mighty) · [full_default](../../stacks/full_default/README.md) · [full_exploration](../../stacks/full_exploration/README.md) · [full_raven](../../stacks/full_raven/README.md) | | [optitrack](optitrack.md) | OptiTrack NatNet mocap integration — natnet_ros2 client + PX4 external-vision fusion bridges on the robot, and the Motive-compatible NatNet server emulator for Isaac Sim | `ros_package` | ajong@andrew.cmu.edu | `>=0.19.0-alpha.18 <0.20.0` | [repo](https://github.com/castacks/asm_optitrack) | +| [raven](raven.md) | RAVEN semantic aerial navigation (ICRA 2026) — the RayFronts open-set semantic ray-frontier map and behavior manager (voxel, ray, LVLM and frontier search) in a GPU sidecar container publishing global_plan toward a natural-language target, plus the raven_bridge plan/prompt relay on the robot | `ros_package` | ajong@andrew.cmu.edu | `>=0.21.0-dev.8 <0.22.0` | [repo](https://github.com/castacks/asm_raven) · [full_raven](../../stacks/full_raven/README.md) | ## Registered stacks @@ -37,7 +39,9 @@ The stacks below are the ones REGISTERED in the index; the site nav's | [full_default](../../stacks/full_default/README.md) | The full-autonomy topology as a self-contained stack folder, with the MIGHTY map-based local planner (asm_mighty module: planner + acl-mapping voxel world model + NavigateTask bridge) — the baseline most users start from, the default when no stack is selected, and the stack other stacks are copied from | `>=0.21.0-dev.10 <0.22.0` | [wiring.md](../../stacks/full_default/wiring.md) | [full_default.yaml](https://github.com/castacks/airstack-modules-index/blob/main/stacks/full_default.yaml) | | [full_droan](../../stacks/full_droan/README.md) | Full autonomy with the GPU DROAN reactive local planner (droan_gl, asm_droan module) in place of the default MIGHTY planner — the 0.20.x full_default topology kept as a named stack for depth-camera-only vehicles and legacy comparisons | `>=0.21.0-dev.10 <0.22.0` | [wiring.md](../../stacks/full_droan/wiring.md) | [full_droan.yaml](https://github.com/castacks/airstack-modules-index/blob/main/stacks/full_droan.yaml) | | [full_droan_cpu](../../stacks/full_droan_cpu/README.md) | Full autonomy with the CPU DROAN local planner (droan_local_planner + live disparity_expansion world model, asm_droan module) in place of the default MIGHTY planner | `>=0.21.0-dev.10 <0.22.0` | [wiring.md](../../stacks/full_droan_cpu/wiring.md) | [full_droan_cpu.yaml](https://github.com/castacks/airstack-modules-index/blob/main/stacks/full_droan_cpu.yaml) | +| [full_exploration](../../stacks/full_exploration/README.md) | Full autonomy (full_default topology: MIGHTY LiDAR-based local planner) with the frontier-based geometric exploration planner (exploration_planner module) in place of the random walk; everything else is identical to full_default | `>=0.21.0-dev.8 <0.22.0` | *not committed yet* | [full_exploration.yaml](https://github.com/castacks/airstack-modules-index/blob/main/stacks/full_exploration.yaml) | | [full_macvo](../../stacks/full_macvo/README.md) | Full autonomy with MAC-VO learned stereo visual odometry as the disparity source for the DROAN local planner (droan_gl, asm_droan module, consumes /$ROBOT_NAME/perception/macvo/disparity) | `>=0.21.0-dev.10 <0.22.0` | [wiring.md](../../stacks/full_macvo/wiring.md) | [full_macvo.yaml](https://github.com/castacks/airstack-modules-index/blob/main/stacks/full_macvo.yaml) | +| [full_raven](../../stacks/full_raven/README.md) | Full autonomy (full_default topology: MIGHTY LiDAR-based local planner) with RAVEN open-set semantic navigation as the global planner (RayFronts GPU sidecar container + raven_bridge); everything else is identical to full_default | `>=0.21.0-dev.8 <0.22.0` | *not committed yet* | [full_raven.yaml](https://github.com/castacks/airstack-modules-index/blob/main/stacks/full_raven.yaml) | ## See also diff --git a/docs/modules/mighty.md b/docs/modules/mighty.md index 5d960517e..bec323108 100644 --- a/docs/modules/mighty.md +++ b/docs/modules/mighty.md @@ -50,6 +50,8 @@ matrix, read the declaration as intent. ## Registered stacks using this module - [full_default](../../stacks/full_default/README.md) +- [full_exploration](../../stacks/full_exploration/README.md) +- [full_raven](../../stacks/full_raven/README.md) ## Registry notes diff --git a/docs/modules/raven.md b/docs/modules/raven.md new file mode 100644 index 000000000..b492f8df7 --- /dev/null +++ b/docs/modules/raven.md @@ -0,0 +1,56 @@ +# raven + + + +> RAVEN semantic aerial navigation (ICRA 2026) — the RayFronts open-set semantic ray-frontier map and behavior manager (voxel, ray, LVLM and frontier search) in a GPU sidecar container publishing global_plan toward a natural-language target, plus the raven_bridge plan/prompt relay on the robot + +| | | +|---|---| +| Repository | [castacks/asm_raven](https://github.com/castacks/asm_raven) | +| Type | `ros_package` | +| Maintainer | ajong@andrew.cmu.edu | +| License | BSD-3-Clause-Clear | +| Registered ref | [`v0.1.1`](https://github.com/castacks/asm_raven/tree/v0.1.1) | +| Declared compat | `>=0.21.0-dev.8 <0.22.0` | +| Registry entry | [modules/raven.yaml](https://github.com/castacks/airstack-modules-index/blob/main/modules/raven.yaml) | + +## Install + +From an AirStack checkout ([AirStack Modules guide](../development/modules.md)): + +```bash +airstack module add https://github.com/castacks/asm_raven --version v0.1.1 +airstack up +``` + +`module add` pins the module in `modules.repos` and syncs it into the +gitignored `modules/` overlay; `airstack up` automatically includes the +generated compose override that mounts it into the containers. + +## Compatibility: declared vs verified + +The range `>=0.21.0-dev.8 <0.22.0` is **DECLARED** by the module author +(copied from the module's `module.yaml`). The **VERIFIED** record — rows +stamped exclusively by CI runs of the reusable +[module-system-tests workflow](../development/module_ci.md) — lives in the +registry's [compat/ matrix](https://github.com/castacks/airstack-modules-index/tree/main/compat) +([compat/raven.yaml](https://github.com/castacks/airstack-modules-index/blob/main/compat/raven.yaml) once stamped). +A compatibility claim that isn't CI-verified rots: trust the +matrix, read the declaration as intent. + +## Documentation + +- [Module README on GitHub @ `v0.1.1`](https://github.com/castacks/asm_raven/blob/v0.1.1/README.md) +- *The module repo was not fetched when this page was generated — the* + *links above go to GitHub at the registered ref (failure isolation:* + *an unreachable module repo never fails the docs deploy).* + +## Registered stacks using this module + +- [full_raven](../../stacks/full_raven/README.md) + +## Registry notes + +> Wrapper (raven_bridge, sidecar Dockerfile, compose fragment, test_stack) is BSD-3-Clause-Clear; the vendored RayFronts fork (seungchan-kim/RayFronts, branch raven) keeps its own license — see the module's VENDORED.md. The sidecar is declared through the manifest's compose fragment (first module to bring its own container) and is built on first `airstack up` (~20 GB image). Consumed by trunk reference stack full_raven. diff --git a/docs/release_notes/index.md b/docs/release_notes/index.md index 4a73db2ad..7af24df9d 100644 --- a/docs/release_notes/index.md +++ b/docs/release_notes/index.md @@ -19,6 +19,34 @@ its own notes. --> ## 0.21.0 (Unreleased) +- **New modules: RAVEN semantic planner and the geometric exploration + planner.** Two global planners now ship as external modules and pinned + reference stacks: [asm_raven](https://github.com/castacks/asm_raven) + (RAVEN, ICRA 2026 — RayFronts open-set semantic ray-frontier map + behavior + manager in a GPU sidecar container, `raven_bridge` on the robot; stack + `full_raven`) and + [asm_exploration_planner](https://github.com/castacks/asm_exploration_planner) + (the frontier-based geometric exploration planner from the construction-site + project, extracted from trunk's `robot/ros_ws/src/global/planners/exploration` + with its bounded viewpoint sampling; stack `full_exploration`). Both stacks + are one `` swap against `full_default`: the MIGHTY bridge follows + the published `global_plan` directly, and its LiDAR-based world model keeps + working in unlit scenes where stereo disparity does not. +- **`global_plan_navigate_bridge`.** New trunk package that turns a + `global_plan` topic into `NavigateTask` goals (cancel-then-resend on each new + plan), so global planners that only publish a path can drive a + task-executor local planner such as `droan_gl` (`asm_droan`, stacks + `full_droan*`); the MIGHTY bridge follows the topic itself. `full_default`'s random walk remains its + own action client. +- **Compose fragments may declare sidecar services with a `build:` section.** + `airstack module sync` now rewrites a module compose fragment's relative + `build.context` / `build.dockerfile` / `env_file` paths to absolute, like it + already did for bind-mount sources, so a module can bring its own container + (the RAVEN sidecar is the first). +- **Headless follow-camera video.** `ISAAC_SIM_FOLLOW_CAM_TOPIC` publishes the + Isaac Sim follow camera as a raw `sensor_msgs/Image` topic + (`ISAAC_SIM_FOLLOW_CAM_RES`, default 1280x720), giving headless runs and CI a + third-person video feed. - **asm_mighty v0.1.5: follower fixes for the default planner.** The `full_default`, `lite_default` and `lite_offload_global` stacks pin [`mighty`](../modules/mighty.md) v0.1.5. Its `mighty_bridge` global_plan diff --git a/docs/robot/autonomy/global/planning/index.md b/docs/robot/autonomy/global/planning/index.md index 2da25a8e2..a9c725309 100644 --- a/docs/robot/autonomy/global/planning/index.md +++ b/docs/robot/autonomy/global/planning/index.md @@ -91,13 +91,20 @@ that generates a plan by randomly selecting a direction to move in, and is useful for testing the robot's ability to follow a plan. It is the reference task-executor implementation, serving `tasks/exploration`. -### Exploration planner - -The [exploration planner](../../../../../robot/ros_ws/src/global/planners/exploration/README.md) -(`robot/ros_ws/src/global/planners/exploration`) is a frontier-based -geometric exploration planner — an alternative to `random_walk`. To use it, -swap the `random_walk_planner.launch.xml` include in your stack's entry -launch file for `exploration_launch.xml`, as described in its README. +### Exploration planner (module) + +The [asm_exploration_planner](https://github.com/castacks/asm_exploration_planner) +module provides a frontier-based geometric exploration planner: it builds its +own OpenVDB occupancy map from the LiDAR cloud, extracts and clusters +frontiers, samples viewpoints around them (optionally confined to a bounding +box), and publishes a shortened, collision-checked RRT path on `global_plan`. +The [`full_exploration`](https://github.com/castacks/AirStack/tree/develop/stacks/full_exploration) +reference stack selects it: `full_default` with the random-walk include +replaced by the module's `exploration_planner.launch.xml`, followed by the +[`global_plan_navigate_bridge`](../../../../../robot/ros_ws/src/global/global_plan_navigate_bridge/README.md) +that turns each published plan into a `NavigateTask` goal for the local +planner. Run it with `airstack module sync` (the stack pins the module) and +`airstack up --stack full_exploration --sim isaac`. ## Writing Your Own Global Planner diff --git a/docs/robot/autonomy_modes.md b/docs/robot/autonomy_modes.md index 1cc0a2214..6b836ac70 100644 --- a/docs/robot/autonomy_modes.md +++ b/docs/robot/autonomy_modes.md @@ -14,6 +14,8 @@ environment variable is a preflight error.) | **`lite_offload_global:offboard`** | Global planner + world model only — runs on the GCS paired with onboard robots | | **`full_droan`** | `full_default` with the GPU DROAN reactive local planner (`droan_gl`, `asm_droan` module) in place of MIGHTY | | **`full_droan_cpu`** | `full_default` with the CPU DROAN local planner (`droan_local_planner` + `disparity_expansion`, `asm_droan` module) in place of MIGHTY | +| **`full_exploration`** | `full_default` with the frontier-based geometric exploration planner (`asm_exploration_planner` module) in place of the random walk; MIGHTY's bridge follows the published `global_plan` | +| **`full_raven`** | `full_default` with RAVEN semantic navigation (`asm_raven` module: RayFronts GPU sidecar + `raven_bridge`) as the global planner — fly toward a natural-language target | | **`full_macvo`** | `full_droan` with MAC-VO as the planner's disparity source (`asm_macvo` + `asm_droan` modules) | Instead of picking a stack per container, `airstack up --fleet ` launches diff --git a/docs/robot/configuration/environment_variables.md b/docs/robot/configuration/environment_variables.md index 02886d0f4..3fe239ed2 100644 --- a/docs/robot/configuration/environment_variables.md +++ b/docs/robot/configuration/environment_variables.md @@ -78,7 +78,7 @@ These appear commented-out in `.env` (or are read by compose with a built-in def | `MS_AIRSIM_SCENE`, `MS_AIRSIM_ENV_DIR`, `MS_AIRSIM_BINARY_PATH`, `MS_AIRSIM_HEADLESS`, `MS_AIRSIM_PX4_START_DELAY` | UE4 scene fetching/selection, binary override, off-screen rendering, PX4 start delay | [MS AirSim Docker](../../simulation/ms-airsim/docker.md) | | `BAG_STORAGE_PATH` | Host directory mounted at `/bags` on the `l4t` profile (default `/media/airlab/Storage/airstack_collection`) | [Rosbags](../logging/rosbags.md) | | `LOG_CONFIG` | Bag recorder topic-selection file in `logging_bringup/config/` (default `log.yaml`) | [Rosbags](../logging/rosbags.md) | -| `ISAAC_SIM_FOLLOW_CAM`, `ISAAC_SIM_FOLLOW_CAM_OFFSET`, `ISAAC_SIM_FOLLOW_CAM_LIGHT` | Viewport follow-camera: drone domain id to chase, world-frame offset, headlight for unlit interiors | [Isaac Sim](../../simulation/isaac_sim/index.md) (read by `pegasus_app.py`) | +| `ISAAC_SIM_FOLLOW_CAM`, `ISAAC_SIM_FOLLOW_CAM_OFFSET`, `ISAAC_SIM_FOLLOW_CAM_LIGHT`, `ISAAC_SIM_FOLLOW_CAM_TOPIC`, `ISAAC_SIM_FOLLOW_CAM_RES` | Viewport follow-camera: drone domain id to chase, world-frame offset, headlight for unlit interiors; optionally publish the camera as a raw `sensor_msgs/Image` topic (`WxH`, default 1280x720) for headless video capture | [Isaac Sim](../../simulation/isaac_sim/index.md) (read by `pegasus_app.py`) | | `SIM_IP` | Address robot containers use to reach the simulator (default `172.31.0.200`; every sim service binds this fixed address) | `robot/docker/docker-compose.yaml`, `simulation/isaac-sim/docker/docker-compose.yaml` | | `FCU_URL` | MAVROS flight-controller URL on real hardware (default `/dev/ttyTHS4:115200` on `l4t`); desktop/sim derive it from the base ports instead | `robot/docker/docker-compose.yaml` (`robot-l4t`) | | `CACHE_TAG` | Floating Docker-layer-cache image tag (default `cache`) — CI only | [CI/CD](../../development/intermediate/testing/ci_cd.md) | diff --git a/mkdocs.yml b/mkdocs.yml index f11adee65..f3d42d677 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -188,7 +188,9 @@ nav: - full_default: stacks/full_default/README.md - full_droan: stacks/full_droan/README.md - full_droan_cpu: stacks/full_droan_cpu/README.md + - full_exploration: stacks/full_exploration/README.md - full_macvo: stacks/full_macvo/README.md + - full_raven: stacks/full_raven/README.md - lite_default: stacks/lite_default/README.md - lite_offload_global: stacks/lite_offload_global/README.md - Modules Catalog: @@ -196,9 +198,11 @@ nav: - module.yaml Schema: common/module_schema/README.md - dfm2_disturbances: docs/modules/dfm2_disturbances.md - droan: docs/modules/droan.md + - exploration_planner: docs/modules/exploration_planner.md - macvo: docs/modules/macvo.md - mighty: docs/modules/mighty.md - optitrack: docs/modules/optitrack.md + - raven: docs/modules/raven.md - Autonomy Packages: - Robot Overview: docs/robot/index.md - Autonomy Overview: docs/robot/autonomy/index.md @@ -224,7 +228,7 @@ nav: - VDB Mapping: robot/ros_ws/src/global/world_models/vdb_mapping_ros2/README.md - Planning: docs/robot/autonomy/global/planning/index.md - Random Walk: robot/ros_ws/src/global/planners/random_walk/README.md - - Exploration: robot/ros_ws/src/global/planners/exploration/README.md + - Global Plan Navigate Bridge: robot/ros_ws/src/global/global_plan_navigate_bridge/README.md - Behavior: docs/robot/autonomy/behavior/index.md - Coordination: docs/robot/autonomy/coordination/index.md - RViz Tasks Panel: common/ros_packages/gui/rviz/rviz_tasks_panel/README.md diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/README.md b/robot/ros_ws/src/global/global_plan_navigate_bridge/README.md new file mode 100644 index 000000000..f56fd01f9 --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/README.md @@ -0,0 +1,79 @@ +# global_plan_navigate_bridge + +Adapter between a **topic-publishing global planner** and AirStack's +**task-executor local planner**. The DROAN local planner (`droan_gl`, from the +`asm_droan` module used by the `full_droan*` stacks) plans only while a +`NavigateTask` action goal is active, and it rejects a second goal while one +is running. The default MIGHTY local planner (`full_default`) instead follows +the `global_plan` topic itself and needs no adapter. The random-walk planner drives it as an action client +itself; planners that simply publish a `nav_msgs/Path` on `global_plan` — the +[exploration_planner](https://github.com/castacks/asm_exploration_planner) +and [raven](https://github.com/castacks/asm_raven) modules, or any external +planner following the interface conventions — need this bridge to make the +vehicle move. + +## Architecture + +```mermaid +flowchart LR + GP[global planner
exploration_planner / raven / ...] -->|"global_plan (nav_msgs/Path)"| B[global_plan_navigate_bridge] + B -->|"NavigateTask goal
(cancel active, then send)"| LP[droan_gl
tasks/navigate] + LP -->|trajectory_segment_to_add| TC[trajectory_controller] +``` + +Behaviour: + +1. A new plan whose **pose count** or **final pose** changed (by more than + `replan_min_change_m`) cancels the active goal and, once the cancel is + acknowledged, sends the new plan as a `NavigateTask` goal. `droan_gl` may + still report "task already active" for a moment after the cancel, so the + send is retried (`send_retry_period_s`, `max_send_retries`). +2. Identical re-publications (planners typically re-send their current plan + periodically) are ignored. +3. An empty `Path` cancels the active goal (`cancel_on_empty_plan`). +4. `~/set_enabled` (`std_srvs/SetBool`) pauses/resumes the bridge; disabling + cancels the active goal. + +## Interfaces + +Declared in `launch/global_plan_navigate_bridge.launch.xml` (canonical +defaults; rewire from the stack entry file only): + +| Launch arg | Default | Direction | Type | +|---|---|---|---| +| `global_plan_navigate_bridge_global_plan_topic` | `/$ROBOT_NAME/global_plan` | in | `nav_msgs/Path` | +| `global_plan_navigate_bridge_navigate_task_action` | `/$ROBOT_NAME/tasks/navigate` | client | `task_msgs/action/NavigateTask` | +| `global_plan_navigate_bridge_config` | `config/global_plan_navigate_bridge.yaml` | — | parameter file | + +Service: `/$ROBOT_NAME/global_plan_navigate_bridge/set_enabled` (`std_srvs/SetBool`). + +## Parameters + +| Parameter | Default | Description | +|---|---|---| +| `goal_tolerance_m` | `1.0` | Passed through in every `NavigateTask` goal | +| `replan_min_change_m` | `0.5` | Final-pose displacement below which a re-published plan is treated as unchanged | +| `cancel_on_empty_plan` | `true` | An empty `Path` cancels the active goal | +| `send_retry_period_s` | `0.5` | Retry period after a rejected send | +| `max_send_retries` | `10` | Give up (drop the plan) after this many rejections | +| `enabled` | `true` | Initial state of the `set_enabled` switch | + +## Usage + +Include it in a DROAN-based stack whose global planner publishes plans on a +topic, right after that planner's include (the MIGHTY-based `full_exploration` +and `full_raven` stacks do not need it): + +```xml + +``` + +`full_default` does not include it: its random-walk planner is its own +`NavigateTask` client and MIGHTY follows `global_plan` directly. + +## Testing + +Hermetic unit tests for the plan-change rule live in `test/` and run with +`airstack test -m unit` (listed in `tests/colcon_unit_test_packages.yaml`) or +`colcon test --packages-select global_plan_navigate_bridge` in the robot +container. diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/config/global_plan_navigate_bridge.yaml b/robot/ros_ws/src/global/global_plan_navigate_bridge/config/global_plan_navigate_bridge.yaml new file mode 100644 index 000000000..4abc98c11 --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/config/global_plan_navigate_bridge.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + # Passed through in every NavigateTask goal (m). + goal_tolerance_m: 1.0 + # A new plan replaces the active goal only when its final pose moved more + # than this (m) or its pose count changed; identical re-publications are + # ignored so a planner re-sending its plan at 1 Hz does not thrash droan_gl. + replan_min_change_m: 0.5 + # An empty Path cancels the active goal (planners publish an empty plan to + # stop the vehicle). + cancel_on_empty_plan: true + # droan_gl REJECTS a goal while one is still active; after a cancel the + # bridge retries the send at this period, up to max_send_retries times. + send_retry_period_s: 0.5 + max_send_retries: 10 + # Start enabled; toggle at runtime with the ~/set_enabled std_srvs/SetBool + # service (disabling cancels the active goal). + enabled: true diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/__init__.py b/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/__init__.py new file mode 100644 index 000000000..86f74ac2a --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/__init__.py @@ -0,0 +1,3 @@ +# Copyright (c) 2026 Carnegie Mellon University +# SPDX-License-Identifier: BSD-3-Clause-Clear +"""global_plan topic -> NavigateTask goal adapter (see bridge_node.py).""" diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/bridge_node.py b/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/bridge_node.py new file mode 100644 index 000000000..ba1f71392 --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/bridge_node.py @@ -0,0 +1,188 @@ +# Copyright (c) 2026 Carnegie Mellon University +# SPDX-License-Identifier: BSD-3-Clause-Clear +"""global_plan (nav_msgs/Path) -> NavigateTask goal adapter. + +The trunk local planner (droan_gl) is a task executor: it only plans while a +NavigateTask goal is active and it REJECTS a new goal while one is running. +The random-walk planner drives it as an action client itself, but planners +that just publish a path on the ``global_plan`` topic (the exploration_planner +and raven modules, any external planner following the interface conventions) +need this adapter: + + global planner --global_plan (Path)--> bridge --NavigateTask--> droan_gl + +Behaviour: + * a new plan whose pose count or final pose changed (``replan_min_change_m``) + cancels the active goal and, once the cancel is acknowledged, sends the new + plan as a goal (with retries: droan_gl briefly still reports "active"); + * identical re-publications are ignored; + * an empty Path cancels the active goal (``cancel_on_empty_plan``); + * ``~/set_enabled`` (std_srvs/SetBool) pauses/resumes; disabling cancels. +""" +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from nav_msgs.msg import Path +from std_srvs.srv import SetBool +from task_msgs.action import NavigateTask + +from global_plan_navigate_bridge.plan_key import plan_changed, plan_key + + +class GlobalPlanNavigateBridge(Node): + def __init__(self): + super().__init__('global_plan_navigate_bridge') + self.declare_parameter('goal_tolerance_m', 1.0) + self.declare_parameter('replan_min_change_m', 0.5) + self.declare_parameter('cancel_on_empty_plan', True) + self.declare_parameter('send_retry_period_s', 0.5) + self.declare_parameter('max_send_retries', 10) + self.declare_parameter('enabled', True) + + self._enabled = bool(self.get_parameter('enabled').value) + self._client = ActionClient(self, NavigateTask, 'navigate_task') + self._sub = self.create_subscription(Path, 'global_plan', self._on_plan, 10) + self._srv = self.create_service(SetBool, '~/set_enabled', self._on_set_enabled) + + self._active_handle = None # ClientGoalHandle of the running goal + self._active_key = None # plan_key of the running/sending goal + self._pending = None # (Path, key) waiting to be sent + self._sending = False + self._retries = 0 + self._retry_timer = None + self.get_logger().info( + 'global_plan -> NavigateTask bridge ready ' + f'(enabled={self._enabled}, goal_tolerance_m=' + f'{self.get_parameter("goal_tolerance_m").value})') + + # ------------------------------------------------------------- inputs + def _on_set_enabled(self, req, resp): + self._enabled = bool(req.data) + if not self._enabled: + self._pending = None + self._cancel_active() + resp.success = True + resp.message = 'enabled' if self._enabled else 'disabled (active goal cancelled)' + self.get_logger().info(f'set_enabled -> {resp.message}') + return resp + + def _on_plan(self, msg: Path): + if not self._enabled: + return + pts = [(p.pose.position.x, p.pose.position.y, p.pose.position.z) for p in msg.poses] + key = plan_key(pts) + min_change = float(self.get_parameter('replan_min_change_m').value) + current = self._pending[1] if self._pending else self._active_key + if not plan_changed(current, key, min_change): + return + if key is None: + if bool(self.get_parameter('cancel_on_empty_plan').value): + self.get_logger().info('empty global_plan -> cancelling active NavigateTask') + self._pending = None + self._cancel_active() + return + self.get_logger().info( + f'new global_plan ({key[0]} poses, goal {tuple(round(v, 2) for v in key[1])}) ' + '-> replacing NavigateTask goal') + self._pending = (msg, key) + self._dispatch() + + # ------------------------------------------------------------ dispatch + def _dispatch(self): + if self._pending is None or self._sending: + return + if self._active_handle is not None: + self._cancel_active() # _send runs from the cancel callback + return + self._send() + + def _cancel_active(self): + handle = self._active_handle + if handle is None: + return + self._active_handle = None + self._active_key = None + fut = handle.cancel_goal_async() + fut.add_done_callback(lambda _f: self._dispatch()) + + def _send(self): + if self._pending is None: + return + if not self._client.server_is_ready(): + self.get_logger().warn('NavigateTask server not available yet; retrying', throttle_duration_sec=5.0) + self._schedule_retry() + return + path, key = self._pending + goal = NavigateTask.Goal() + goal.global_plan = path + goal.goal_tolerance_m = float(self.get_parameter('goal_tolerance_m').value) + self._sending = True + self._active_key = key + fut = self._client.send_goal_async(goal, feedback_callback=self._on_feedback) + fut.add_done_callback(self._on_goal_response) + + def _schedule_retry(self): + max_retries = int(self.get_parameter('max_send_retries').value) + if self._retries >= max_retries: + self.get_logger().error(f'NavigateTask goal rejected {self._retries} times; dropping plan') + self._pending = None + self._retries = 0 + self._active_key = None + return + self._retries += 1 + period = float(self.get_parameter('send_retry_period_s').value) + if self._retry_timer is not None: + self._retry_timer.cancel() + self._retry_timer = self.create_timer(period, self._on_retry) + + def _on_retry(self): + if self._retry_timer is not None: + self._retry_timer.cancel() + self._retry_timer = None + self._send() + + def _on_goal_response(self, fut): + self._sending = False + handle = fut.result() + if handle is None or not handle.accepted: + self.get_logger().warn('NavigateTask goal rejected (local planner still busy?) - retrying') + self._schedule_retry() + return + self._retries = 0 + self._active_handle = handle + self._pending = None + handle.get_result_async().add_done_callback( + lambda f, h=handle: self._on_result(f, h)) + self.get_logger().info('NavigateTask goal accepted') + + def _on_feedback(self, fb): + self.get_logger().debug( + f'navigate: {fb.feedback.status} dist={fb.feedback.distance_to_goal:.2f} m') + + def _on_result(self, fut, handle): + try: + result = fut.result().result + msg = f'success={result.success} "{result.message}"' + except Exception as exc: # noqa: BLE001 - report whatever the server returned + msg = f'error: {exc}' + self.get_logger().info(f'NavigateTask finished: {msg}') + if self._active_handle is handle: + self._active_handle = None + self._active_key = None + self._dispatch() + + +def main(args=None): + rclpy.init(args=args) + node = GlobalPlanNavigateBridge() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/plan_key.py b/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/plan_key.py new file mode 100644 index 000000000..c13b78ec2 --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/global_plan_navigate_bridge/plan_key.py @@ -0,0 +1,33 @@ +# Copyright (c) 2026 Carnegie Mellon University +# SPDX-License-Identifier: BSD-3-Clause-Clear +"""Pure-python plan comparison used by the bridge (hermetic, no ROS import). + +A global planner typically re-publishes its current plan periodically; the +bridge must only replace the active NavigateTask goal when the plan really +changed, otherwise droan_gl would be cancelled and restarted every tick. +""" +import math +from typing import Optional, Sequence, Tuple + +PlanKey = Tuple[int, Tuple[float, float, float]] + + +def plan_key(points: Sequence[Tuple[float, float, float]]) -> Optional[PlanKey]: + """(pose count, final xyz) of a plan, or None for an empty plan.""" + if not points: + return None + x, y, z = points[-1] + return (len(points), (float(x), float(y), float(z))) + + +def plan_changed(prev: Optional[PlanKey], new: Optional[PlanKey], min_change_m: float) -> bool: + """True when ``new`` should replace the goal built from ``prev``. + + Rules: empty<->non-empty is always a change; a different pose count is a + change; otherwise the final pose must have moved more than ``min_change_m``. + """ + if prev is None or new is None: + return prev is not new and (prev is None) != (new is None) + if prev[0] != new[0]: + return True + return math.dist(prev[1], new[1]) > min_change_m diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/launch/global_plan_navigate_bridge.launch.xml b/robot/ros_ws/src/global/global_plan_navigate_bridge/launch/global_plan_navigate_bridge.launch.xml new file mode 100644 index 000000000..8dc83018e --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/launch/global_plan_navigate_bridge.launch.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/package.xml b/robot/ros_ws/src/global/global_plan_navigate_bridge/package.xml new file mode 100644 index 000000000..73dd84f82 --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/package.xml @@ -0,0 +1,23 @@ + + + + global_plan_navigate_bridge + 0.1.0 + Adapter from a topic-publishing global planner (nav_msgs/Path on global_plan) to the task-executor local planner: each new plan becomes a NavigateTask goal (cancel-then-resend), so planners that only publish paths (exploration_planner, raven) can drive a task-executor local planner such as droan_gl (asm_droan). + Andrew Jong + BSD-3-Clause-Clear + + rclpy + nav_msgs + std_srvs + task_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/resource/global_plan_navigate_bridge b/robot/ros_ws/src/global/global_plan_navigate_bridge/resource/global_plan_navigate_bridge new file mode 100644 index 000000000..e69de29bb diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/setup.cfg b/robot/ros_ws/src/global/global_plan_navigate_bridge/setup.cfg new file mode 100644 index 000000000..b00240ebd --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/setup.cfg @@ -0,0 +1,20 @@ +[develop] +script_dir=$base/lib/global_plan_navigate_bridge +[install] +install_scripts=$base/lib/global_plan_navigate_bridge + +[tool:pytest] +testpaths = test +python_files = test_*.py +python_classes = Test* +python_functions = test_* +markers = + unit: Hermetic unit tests (no ROS stack required) + linter: ament copyright/flake8/pep257 (run separately, not via colcon test) + copyright: ament_copyright + flake8: ament_flake8 + pep257: ament_pep257 +addopts = + --ignore=test/test_copyright.py + --ignore=test/test_flake8.py + --ignore=test/test_pep257.py diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/setup.py b/robot/ros_ws/src/global/global_plan_navigate_bridge/setup.py new file mode 100644 index 000000000..17b09b674 --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup + +package_name = 'global_plan_navigate_bridge' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', ['launch/global_plan_navigate_bridge.launch.xml']), + ('share/' + package_name + '/config', ['config/global_plan_navigate_bridge.yaml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Andrew Jong', + maintainer_email='ajong@andrew.cmu.edu', + description='global_plan topic -> NavigateTask goal adapter for task-executor local planners', + license='BSD-3-Clause-Clear', + extras_require={ + 'test': ['pytest'], + }, + entry_points={ + 'console_scripts': [ + 'global_plan_navigate_bridge_node = global_plan_navigate_bridge.bridge_node:main', + ], + }, +) diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/test/conftest.py b/robot/ros_ws/src/global/global_plan_navigate_bridge/test/conftest.py new file mode 100644 index 000000000..019c78b50 --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/test/conftest.py @@ -0,0 +1,6 @@ +# Skip ament linter modules during pytest/colcon test. +collect_ignore = [ + "test_copyright.py", + "test_flake8.py", + "test_pep257.py", +] diff --git a/robot/ros_ws/src/global/global_plan_navigate_bridge/test/test_plan_key.py b/robot/ros_ws/src/global/global_plan_navigate_bridge/test/test_plan_key.py new file mode 100644 index 000000000..f66ba76ee --- /dev/null +++ b/robot/ros_ws/src/global/global_plan_navigate_bridge/test/test_plan_key.py @@ -0,0 +1,46 @@ +# Copyright (c) 2026 Carnegie Mellon University +# SPDX-License-Identifier: BSD-3-Clause-Clear +"""Hermetic tests for the bridge's plan-change rule (no ROS required).""" +import pytest + +from global_plan_navigate_bridge.plan_key import plan_changed, plan_key + +pytestmark = pytest.mark.unit + + +def test_empty_plan_has_no_key(): + assert plan_key([]) is None + + +def test_key_is_count_and_final_pose(): + assert plan_key([(0, 0, 0), (1.0, 2.0, 3.0)]) == (2, (1.0, 2.0, 3.0)) + + +def test_republished_identical_plan_is_not_a_change(): + k = plan_key([(0, 0, 0), (5, 0, 2)]) + assert not plan_changed(k, k, 0.5) + + +def test_small_goal_jitter_is_not_a_change(): + a = plan_key([(0, 0, 0), (5.0, 0.0, 2.0)]) + b = plan_key([(0, 0, 0), (5.2, 0.1, 2.0)]) + assert not plan_changed(a, b, 0.5) + + +def test_moved_goal_is_a_change(): + a = plan_key([(0, 0, 0), (5.0, 0.0, 2.0)]) + b = plan_key([(0, 0, 0), (9.0, 0.0, 2.0)]) + assert plan_changed(a, b, 0.5) + + +def test_pose_count_change_is_a_change(): + a = plan_key([(0, 0, 0), (5.0, 0.0, 2.0)]) + b = plan_key([(0, 0, 0), (2, 0, 2), (5.0, 0.0, 2.0)]) + assert plan_changed(a, b, 0.5) + + +def test_empty_transitions_are_changes(): + k = plan_key([(1, 1, 1)]) + assert plan_changed(k, None, 0.5) + assert plan_changed(None, k, 0.5) + assert not plan_changed(None, None, 0.5) diff --git a/robot/ros_ws/src/global/planners/exploration/CMakeLists.txt b/robot/ros_ws/src/global/planners/exploration/CMakeLists.txt deleted file mode 100644 index e60aa9f17..000000000 --- a/robot/ros_ws/src/global/planners/exploration/CMakeLists.txt +++ /dev/null @@ -1,147 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules" ${CMAKE_MODULE_PATH}) -project(exploration_planner) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_eigen REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) - -# set(OpenVDB_INCLUDE_DIR "/usr/local/include/openvdb" CACHE STRING "Path to OpenVDB installation") -find_package(OpenVDB REQUIRED) -# find_package(vdb_mapping REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) -find_package(message_filters REQUIRED) - -########### -## Build ## -########### - -# Specify additional locations of header files - -# Declare a C++ executable -add_executable(exploration_planner - src/exploration_node_run.cpp - src/exploration_node.cpp - src/exploration_logic.cpp - src/viewpoint_sampling.cpp - src/collision_checker.cpp - src/rrt_planner.cpp - src/utils.cpp) - -target_include_directories(exploration_planner - PRIVATE - ${PCL_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - ${CMAKE_CURRENT_SOURCE_DIR}/include) - -target_link_libraries(exploration_planner - # vdb_mapping::vdb_mapping - OpenVDB::openvdb - ${EIGEN3_LIBRARIES} - ${PCL_LIBRARIES}) - -add_executable(waypoint_routing_planner - src/waypoint_routing_node.cpp - src/exploration_node.cpp - src/exploration_logic.cpp - src/viewpoint_sampling.cpp - src/collision_checker.cpp - src/rrt_planner.cpp - src/utils.cpp) - -target_include_directories(waypoint_routing_planner - PRIVATE - ${PCL_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - ${CMAKE_CURRENT_SOURCE_DIR}/include) - -target_link_libraries(waypoint_routing_planner - # vdb_mapping::vdb_mapping - OpenVDB::openvdb - ${EIGEN3_LIBRARIES} - ${PCL_LIBRARIES}) - -# Link libraries -ament_target_dependencies(exploration_planner - rclcpp - rclcpp_action - std_msgs - std_srvs - geometry_msgs - nav_msgs - visualization_msgs - tf2 - tf2_ros - pcl_conversions - tf2_eigen - tf2_geometry_msgs -) - -ament_target_dependencies(waypoint_routing_planner - rclcpp - rclcpp_action - std_msgs - std_srvs - geometry_msgs - nav_msgs - visualization_msgs - tf2 - tf2_ros - pcl_conversions - tf2_eigen - tf2_geometry_msgs -) - -# Install executable -install(TARGETS - exploration_planner - waypoint_routing_planner - DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}) - -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME}) - -install(DIRECTORY - src - DESTINATION share/${PROJECT_NAME}) - - -############# -## Testing ## -############# - -# Add gtest based cpp test target and link libraries -# ament_add_gtest(${PROJECT_NAME}-test test/test_exploration_planner.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_node) -# endif() - -################################### -## ament specific configuration ## -################################### - -# ament_package() must be the last call in the file (ament requirement). -ament_package() \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/CMakeModules/FindBlosc.cmake b/robot/ros_ws/src/global/planners/exploration/CMakeModules/FindBlosc.cmake deleted file mode 100644 index 44a0f3f54..000000000 --- a/robot/ros_ws/src/global/planners/exploration/CMakeModules/FindBlosc.cmake +++ /dev/null @@ -1,394 +0,0 @@ -# Copyright Contributors to the OpenVDB Project -# SPDX-License-Identifier: MPL-2.0 -# -#[=======================================================================[.rst: - -FindBlosc ---------- - -Find Blosc include dirs and libraries - -Use this module by invoking find_package with the form:: - - find_package(Blosc - [version] [EXACT] # Minimum or EXACT version e.g. 1.5.0 - [REQUIRED] # Fail with error if Blosc is not found - ) - -IMPORTED Targets -^^^^^^^^^^^^^^^^ - -``Blosc::blosc`` - This module defines IMPORTED target Blosc::Blosc, if Blosc has been found. - -Result Variables -^^^^^^^^^^^^^^^^ - -This will define the following variables: - -``Blosc_FOUND`` - True if the system has the Blosc library. -``Blosc_VERSION`` - The version of the Blosc library which was found. -``Blosc_INCLUDE_DIRS`` - Include directories needed to use Blosc. -``Blosc_RELEASE_LIBRARIES`` - Libraries needed to link to the release version of Blosc. -``Blosc_RELEASE_LIBRARY_DIRS`` - Blosc release library directories. -``Blosc_DEBUG_LIBRARIES`` - Libraries needed to link to the debug version of Blosc. -``Blosc_DEBUG_LIBRARY_DIRS`` - Blosc debug library directories. - -Deprecated - use [RELEASE|DEBUG] variants: - -``Blosc_LIBRARIES`` - Libraries needed to link to Blosc. -``Blosc_LIBRARY_DIRS`` - Blosc library directories. - -Cache Variables -^^^^^^^^^^^^^^^ - -The following cache variables may also be set: - -``Blosc_INCLUDE_DIR`` - The directory containing ``blosc.h``. -``Blosc_LIBRARY`` - The path to the Blosc library. may include target_link_libraries() debug/optimized keywords -``Blosc_LIBRARY_RELEASE`` - The path to the Blosc release library. -``Blosc_LIBRARY_DEBUG`` - The path to the Blosc debug library. - -Hints -^^^^^ - -Instead of explicitly setting the cache variables, the following variables -may be provided to tell this module where to look. - -``Blosc_ROOT`` - Preferred installation prefix. -``BLOSC_INCLUDEDIR`` - Preferred include directory e.g. /include -``BLOSC_LIBRARYDIR`` - Preferred library directory e.g. /lib -``BLOSC_DEBUG_SUFFIX`` - Suffix of the debug version of blosc. Defaults to "_d", OR the empty string for VCPKG_TOOLCHAIN -``SYSTEM_LIBRARY_PATHS`` - Global list of library paths intended to be searched by and find_xxx call -``BLOSC_USE_STATIC_LIBS`` - Only search for static blosc libraries -``BLOSC_USE_EXTERNAL_SOURCES`` - Set to ON if Blosc has been built using external sources for LZ4, snappy, - zlib and zstd. Default is OFF. -``DISABLE_CMAKE_SEARCH_PATHS`` - Disable CMakes default search paths for find_xxx calls in this module - -#]=======================================================================] - -cmake_minimum_required(VERSION 3.15) -include(GNUInstallDirs) - -mark_as_advanced( - Blosc_INCLUDE_DIR - Blosc_LIBRARY -) - -set(_FIND_BLOSC_ADDITIONAL_OPTIONS "") -if(DISABLE_CMAKE_SEARCH_PATHS) - set(_FIND_BLOSC_ADDITIONAL_OPTIONS NO_DEFAULT_PATH) -endif() - -# Set _BLOSC_ROOT based on a user provided root var. Xxx_ROOT and ENV{Xxx_ROOT} -# are prioritised over the legacy capitalized XXX_ROOT variables for matching -# CMake 3.12 behaviour -# @todo deprecate -D and ENV BLOSC_ROOT from CMake 3.12 -if(Blosc_ROOT) - set(_BLOSC_ROOT ${Blosc_ROOT}) -elseif(DEFINED ENV{Blosc_ROOT}) - set(_BLOSC_ROOT $ENV{Blosc_ROOT}) -elseif(BLOSC_ROOT) - set(_BLOSC_ROOT ${BLOSC_ROOT}) -elseif(DEFINED ENV{BLOSC_ROOT}) - set(_BLOSC_ROOT $ENV{BLOSC_ROOT}) -endif() - -# Additionally try and use pkconfig to find blosc -if(USE_PKGCONFIG) - if(NOT DEFINED PKG_CONFIG_FOUND) - find_package(PkgConfig) - endif() - if(PKG_CONFIG_FOUND) - pkg_check_modules(PC_Blosc QUIET blosc) - endif() -endif() - -# ------------------------------------------------------------------------ -# Search for blosc include DIR -# ------------------------------------------------------------------------ - -set(_BLOSC_INCLUDE_SEARCH_DIRS "") -list(APPEND _BLOSC_INCLUDE_SEARCH_DIRS - ${BLOSC_INCLUDEDIR} - ${_BLOSC_ROOT} - ${PC_Blosc_INCLUDE_DIRS} - ${SYSTEM_LIBRARY_PATHS} -) - -# Look for a standard blosc header file. -find_path(Blosc_INCLUDE_DIR blosc.h - ${_FIND_BLOSC_ADDITIONAL_OPTIONS} - PATHS ${_BLOSC_INCLUDE_SEARCH_DIRS} - PATH_SUFFIXES ${CMAKE_INSTALL_INCLUDEDIR} include -) - -if(EXISTS "${Blosc_INCLUDE_DIR}/blosc.h") - file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" - _blosc_version_major_string REGEX "#define BLOSC_VERSION_MAJOR +[0-9]+ " - ) - string(REGEX REPLACE "#define BLOSC_VERSION_MAJOR +([0-9]+).*$" "\\1" - _blosc_version_major_string "${_blosc_version_major_string}" - ) - string(STRIP "${_blosc_version_major_string}" Blosc_VERSION_MAJOR) - - file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" - _blosc_version_minor_string REGEX "#define BLOSC_VERSION_MINOR +[0-9]+ " - ) - string(REGEX REPLACE "#define BLOSC_VERSION_MINOR +([0-9]+).*$" "\\1" - _blosc_version_minor_string "${_blosc_version_minor_string}" - ) - string(STRIP "${_blosc_version_minor_string}" Blosc_VERSION_MINOR) - - file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" - _blosc_version_release_string REGEX "#define BLOSC_VERSION_RELEASE +[0-9]+ " - ) - string(REGEX REPLACE "#define BLOSC_VERSION_RELEASE +([0-9]+).*$" "\\1" - _blosc_version_release_string "${_blosc_version_release_string}" - ) - string(STRIP "${_blosc_version_release_string}" Blosc_VERSION_RELEASE) - - unset(_blosc_version_major_string) - unset(_blosc_version_minor_string) - unset(_blosc_version_release_string) - - set(Blosc_VERSION ${Blosc_VERSION_MAJOR}.${Blosc_VERSION_MINOR}.${Blosc_VERSION_RELEASE}) -endif() - -# ------------------------------------------------------------------------ -# Search for blosc lib DIR -# ------------------------------------------------------------------------ - -set(_BLOSC_LIBRARYDIR_SEARCH_DIRS "") -list(APPEND _BLOSC_LIBRARYDIR_SEARCH_DIRS - ${BLOSC_LIBRARYDIR} - ${_BLOSC_ROOT} - ${PC_Blosc_LIBRARY_DIRS} - ${SYSTEM_LIBRARY_PATHS} -) - -# Library suffix handling - -set(_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES}) - -if(WIN32) - if(BLOSC_USE_STATIC_LIBS) - set(CMAKE_FIND_LIBRARY_SUFFIXES ".lib") - endif() -else() - if(BLOSC_USE_STATIC_LIBS) - set(CMAKE_FIND_LIBRARY_SUFFIXES ".a") - endif() -endif() - -set(Blosc_LIB_COMPONENTS "") -# NOTE: Search for debug version first (see vcpkg hack) -list(APPEND BLOSC_BUILD_TYPES DEBUG RELEASE) - -foreach(BUILD_TYPE ${BLOSC_BUILD_TYPES}) - set(_BLOSC_LIB_NAME blosc) - - set(_BLOSC_CMAKE_IGNORE_PATH ${CMAKE_IGNORE_PATH}) - if(VCPKG_TOOLCHAIN) - # Blosc is installed very strangely in VCPKG (debug/release libs have the - # same name, static build uses external deps, dll doesn't) and blosc itself - # comes with almost zero downstream CMake support for us to detect settings. - # We should not support external package managers in our own modules like - # this, but there doesn't seem to be a work around - if(NOT DEFINED BLOSC_DEBUG_SUFFIX) - set(BLOSC_DEBUG_SUFFIX "") - endif() - if(BUILD_TYPE STREQUAL RELEASE) - if(EXISTS ${Blosc_LIBRARY_DEBUG}) - get_filename_component(_BLOSC_DEBUG_DIR ${Blosc_LIBRARY_DEBUG} DIRECTORY) - list(APPEND CMAKE_IGNORE_PATH ${_BLOSC_DEBUG_DIR}) - endif() - endif() - endif() - - if(BUILD_TYPE STREQUAL DEBUG) - if(NOT DEFINED BLOSC_DEBUG_SUFFIX) - set(BLOSC_DEBUG_SUFFIX _d) - endif() - set(_BLOSC_LIB_NAME "${_BLOSC_LIB_NAME}${BLOSC_DEBUG_SUFFIX}") - endif() - - find_library(Blosc_LIBRARY_${BUILD_TYPE} ${_BLOSC_LIB_NAME} - ${_FIND_BLOSC_ADDITIONAL_OPTIONS} - PATHS ${_BLOSC_LIBRARYDIR_SEARCH_DIRS} - PATH_SUFFIXES ${CMAKE_INSTALL_LIBDIR} lib64 lib - ) - - list(APPEND Blosc_LIB_COMPONENTS ${Blosc_LIBRARY_${BUILD_TYPE}}) - set(CMAKE_IGNORE_PATH ${_BLOSC_CMAKE_IGNORE_PATH}) -endforeach() - -# Reset library suffix - -set(CMAKE_FIND_LIBRARY_SUFFIXES ${_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES}) -unset(_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES) - -if(Blosc_LIBRARY_DEBUG AND Blosc_LIBRARY_RELEASE) - # if the generator is multi-config or if CMAKE_BUILD_TYPE is set for - # single-config generators, set optimized and debug libraries - get_property(_isMultiConfig GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) - if(_isMultiConfig OR CMAKE_BUILD_TYPE) - set(Blosc_LIBRARY optimized ${Blosc_LIBRARY_RELEASE} debug ${Blosc_LIBRARY_DEBUG}) - else() - # For single-config generators where CMAKE_BUILD_TYPE has no value, - # just use the release libraries - set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE}) - endif() - # FIXME: This probably should be set for both cases - set(Blosc_LIBRARIES optimized ${Blosc_LIBRARY_RELEASE} debug ${Blosc_LIBRARY_DEBUG}) -endif() - -# if only the release version was found, set the debug variable also to the release version -if(Blosc_LIBRARY_RELEASE AND NOT Blosc_LIBRARY_DEBUG) - set(Blosc_LIBRARY_DEBUG ${Blosc_LIBRARY_RELEASE}) - set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE}) - set(Blosc_LIBRARIES ${Blosc_LIBRARY_RELEASE}) -endif() - -# if only the debug version was found, set the release variable also to the debug version -if(Blosc_LIBRARY_DEBUG AND NOT Blosc_LIBRARY_RELEASE) - set(Blosc_LIBRARY_RELEASE ${Blosc_LIBRARY_DEBUG}) - set(Blosc_LIBRARY ${Blosc_LIBRARY_DEBUG}) - set(Blosc_LIBRARIES ${Blosc_LIBRARY_DEBUG}) -endif() - -# If the debug & release library ends up being the same, omit the keywords -if("${Blosc_LIBRARY_RELEASE}" STREQUAL "${Blosc_LIBRARY_DEBUG}") - set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE} ) - set(Blosc_LIBRARIES ${Blosc_LIBRARY_RELEASE} ) -endif() - -if(Blosc_LIBRARY) - set(Blosc_FOUND TRUE) -else() - set(Blosc_FOUND FALSE) -endif() - -# ------------------------------------------------------------------------ -# Cache and set Blosc_FOUND -# ------------------------------------------------------------------------ - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(Blosc - FOUND_VAR Blosc_FOUND - REQUIRED_VARS - Blosc_LIBRARY - Blosc_INCLUDE_DIR - VERSION_VAR Blosc_VERSION -) - -if(NOT Blosc_FOUND) - if(Blosc_FIND_REQUIRED) - message(FATAL_ERROR "Unable to find Blosc") - endif() - return() -endif() - -# Partition release/debug lib vars - -set(Blosc_RELEASE_LIBRARIES ${Blosc_LIBRARY_RELEASE}) -get_filename_component(Blosc_RELEASE_LIBRARY_DIRS ${Blosc_RELEASE_LIBRARIES} DIRECTORY) -set(Blosc_DEBUG_LIBRARIES ${Blosc_LIBRARY_DEBUG}) -get_filename_component(Blosc_DEBUG_LIBRARY_DIRS ${Blosc_DEBUG_LIBRARIES} DIRECTORY) -set(Blosc_LIBRARIES ${Blosc_RELEASE_LIBRARIES}) -set(Blosc_LIBRARY_DIRS ${Blosc_RELEASE_LIBRARY_DIRS}) -set(Blosc_INCLUDE_DIRS ${Blosc_INCLUDE_DIR}) -set(Blosc_INCLUDE_DIRS ${Blosc_INCLUDE_DIR}) - -# Configure lib type. If XXX_USE_STATIC_LIBS, we always assume a static -# lib is in use. If win32, we can't mark the import .libs as shared, so -# these are always marked as UNKNOWN. Otherwise, infer from extension. -set(BLOSC_LIB_TYPE UNKNOWN) -if(BLOSC_USE_STATIC_LIBS) - set(BLOSC_LIB_TYPE STATIC) -elseif(UNIX) - get_filename_component(_BLOSC_EXT ${Blosc_LIBRARY_RELEASE} EXT) - if(_BLOSC_EXT STREQUAL ".a") - set(BLOSC_LIB_TYPE STATIC) - elseif(_BLOSC_EXT STREQUAL ".so" OR - _BLOSC_EXT STREQUAL ".dylib") - set(BLOSC_LIB_TYPE SHARED) - endif() -endif() - -get_filename_component(Blosc_LIBRARY_DIRS ${Blosc_LIBRARY_RELEASE} DIRECTORY) - -if(NOT TARGET Blosc::blosc) - add_library(Blosc::blosc ${BLOSC_LIB_TYPE} IMPORTED) - set_target_properties(Blosc::blosc PROPERTIES - INTERFACE_COMPILE_OPTIONS "${PC_Blosc_CFLAGS_OTHER}" - INTERFACE_INCLUDE_DIRECTORIES "${Blosc_INCLUDE_DIRS}") - - # Standard location - set_target_properties(Blosc::blosc PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${Blosc_LIBRARY}") - - # Release location - if(EXISTS "${Blosc_LIBRARY_RELEASE}") - set_property(TARGET Blosc::blosc APPEND PROPERTY - IMPORTED_CONFIGURATIONS RELEASE) - set_target_properties(Blosc::blosc PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX" - IMPORTED_LOCATION_RELEASE "${Blosc_LIBRARY_RELEASE}") - endif() - - # Debug location - if(EXISTS "${Blosc_LIBRARY_DEBUG}") - set_property(TARGET Blosc::blosc APPEND PROPERTY - IMPORTED_CONFIGURATIONS DEBUG) - set_target_properties(Blosc::blosc PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX" - IMPORTED_LOCATION_DEBUG "${Blosc_LIBRARY_DEBUG}") - endif() - - # Blosc may optionally be compiled with external sources for - # lz4, snappy and zlib . Add them as interface libs if requested - # (there doesn't seem to be a way to figure this out automatically). - # We assume they live along side blosc - if(BLOSC_USE_EXTERNAL_SOURCES) - set_target_properties(Blosc::blosc PROPERTIES - INTERFACE_LINK_DIRECTORIES - "\$<\$:${Blosc_RELEASE_LIBRARY_DIRS}>;\$<\$:${Blosc_DEBUG_LIBRARY_DIRS}>") - target_link_libraries(Blosc::blosc INTERFACE - $<$:lz4;snappy;zlib> - $<$:lz4d;snappyd;zlibd>) - - if(BLOSC_USE_STATIC_LIBS) - target_link_libraries(Blosc::blosc INTERFACE - $<$:zstd_static> - $<$:zstd_staticd>) - else() - target_link_libraries(Blosc::blosc INTERFACE - $<$:zstd> - $<$:zstdd>) - endif() - endif() -endif() - diff --git a/robot/ros_ws/src/global/planners/exploration/CMakeModules/FindOpenVDB.cmake b/robot/ros_ws/src/global/planners/exploration/CMakeModules/FindOpenVDB.cmake deleted file mode 100644 index 0dac1de40..000000000 --- a/robot/ros_ws/src/global/planners/exploration/CMakeModules/FindOpenVDB.cmake +++ /dev/null @@ -1,838 +0,0 @@ -# Copyright Contributors to the OpenVDB Project -# SPDX-License-Identifier: MPL-2.0 -# -#[=======================================================================[.rst: - -FindOpenVDB ------------ - -Find OpenVDB include dirs, libraries and settings - -Use this module by invoking find_package with the form:: - - find_package(OpenVDB - [version] [EXACT] # Minimum or EXACT version - [REQUIRED] # Fail with error if OpenVDB is not found - [COMPONENTS ...] # OpenVDB libraries by their canonical name - # e.g. "openvdb" for "libopenvdb", - # "pyopenvdb" for the python plugin - # "openvdb_ax" for the OpenVDB AX extension - # "openvdb_houdini" for the houdini plugin - # "nanovdb" for the nanovdb extension - ) - -IMPORTED Targets -^^^^^^^^^^^^^^^^ - -``OpenVDB::openvdb`` - The core openvdb library target. -``OpenVDB::openvdb_je`` - The core openvdb library target with jemalloc. -``OpenVDB::pyopenvdb`` - The openvdb python library target. -``OpenVDB::openvdb_houdini`` - The openvdb houdini library target. -``OpenVDB::openvdb_ax`` - The openvdb_ax library target. -``OpenVDB::nanovdb`` - The nanovdb library target. - -Result Variables -^^^^^^^^^^^^^^^^ - -This will define the following variables: - -``OpenVDB_FOUND`` - True if the system has the OpenVDB library. -``OpenVDB_VERSION`` - The version of the OpenVDB library which was found. -``OpenVDB_INCLUDE_DIRS`` - Include directories needed to use OpenVDB. -``OpenVDB_LIBRARIES`` - Libraries needed to link to OpenVDB. -``OpenVDB_LIBRARY_DIRS`` - OpenVDB library directories. -``OpenVDB_DEFINITIONS`` - Definitions to use when compiling code that uses OpenVDB. -``OpenVDB_${COMPONENT}_FOUND`` - True if the system has the named OpenVDB component. -``OpenVDB_USES_BLOSC`` - True if the OpenVDB Library has been built with blosc support -``OpenVDB_USES_ZLIB`` - True if the OpenVDB Library has been built with zlib support -``OpenVDB_USES_LOG4CPLUS`` - True if the OpenVDB Library has been built with log4cplus support -``OpenVDB_USES_IMATH_HALF`` - True if the OpenVDB Library has been built with Imath half support -``OpenVDB_ABI`` - Set if this module was able to determine the ABI number the located - OpenVDB Library was built against. Unset otherwise. - -Cache Variables -^^^^^^^^^^^^^^^ - -The following cache variables may also be set: - -``OpenVDB_INCLUDE_DIR`` - The directory containing ``openvdb/version.h``. -``OpenVDB_${COMPONENT}_INCLUDE_DIR`` - Individual component include directories for OpenVDB -``OpenVDB_${COMPONENT}_LIBRARY`` - Individual component libraries for OpenVDB - -Hints -^^^^^ - -Instead of explicitly setting the cache variables, the following variables -may be provided to tell this module where to look. - -``OpenVDB_ROOT`` - Preferred installation prefix. -``OPENVDB_INCLUDEDIR`` - Preferred include directory e.g. /include -``OPENVDB_LIBRARYDIR`` - Preferred library directory e.g. /lib -``OPENVDB_${COMPONENT}_ROOT`` - Preferred installation prefix of a specific component. -``OPENVDB_${COMPONENT}_INCLUDEDIR`` - Preferred include directory of a specific component e.g. /include -``OPENVDB_${COMPONENT}_LIBRARYDIR`` - Preferred library directory of a specific component e.g. /lib -``SYSTEM_LIBRARY_PATHS`` - Global list of library paths intended to be searched by and find_xxx call -``OPENVDB_USE_STATIC_LIBS`` - Only search for static openvdb libraries -``DISABLE_CMAKE_SEARCH_PATHS`` - Disable CMakes default search paths for find_xxx calls in this module - -#]=======================================================================] - -cmake_minimum_required(VERSION 3.15) -include(GNUInstallDirs) - - -# Include utility functions for version information -include(${CMAKE_CURRENT_LIST_DIR}/OpenVDBUtils.cmake) - -mark_as_advanced( - OpenVDB_INCLUDE_DIR - OpenVDB_LIBRARY -) - -set(_FIND_OPENVDB_ADDITIONAL_OPTIONS "") -if(DISABLE_CMAKE_SEARCH_PATHS) - set(_FIND_OPENVDB_ADDITIONAL_OPTIONS NO_DEFAULT_PATH) -endif() - -set(_OPENVDB_COMPONENT_LIST - openvdb - openvdb_je - pyopenvdb - openvdb_ax - openvdb_houdini - nanovdb -) - -if(OpenVDB_FIND_COMPONENTS) - set(OPENVDB_COMPONENTS_PROVIDED TRUE) - set(_IGNORED_COMPONENTS "") - foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) - if(NOT ${COMPONENT} IN_LIST _OPENVDB_COMPONENT_LIST) - list(APPEND _IGNORED_COMPONENTS ${COMPONENT}) - endif() - endforeach() - - if(_IGNORED_COMPONENTS) - message(STATUS "Ignoring unknown components of OpenVDB:") - foreach(COMPONENT ${_IGNORED_COMPONENTS}) - message(STATUS " ${COMPONENT}") - endforeach() - list(REMOVE_ITEM OpenVDB_FIND_COMPONENTS ${_IGNORED_COMPONENTS}) - endif() -else() - set(OPENVDB_COMPONENTS_PROVIDED FALSE) - set(OpenVDB_FIND_COMPONENTS openvdb) -endif() - -# always make sure openvdb is picked up as a component i.e. -# find_package(OpenVDB COMPONENTS pyopenvdb) results in both -# openvdb and pyopenvdb targets. Also make sure it appears -# first in the component lists. -list(INSERT OpenVDB_FIND_COMPONENTS 0 openvdb) -list(REMOVE_DUPLICATES OpenVDB_FIND_COMPONENTS) - -# Set _OPENVDB_ROOT based on a user provided root var. Xxx_ROOT and ENV{Xxx_ROOT} -# are prioritised over the legacy capitalized XXX_ROOT variables for matching -# CMake 3.12 behaviour -# @todo deprecate -D and ENV OPENVDB_ROOT from CMake 3.12 -if(OpenVDB_ROOT) - set(_OPENVDB_ROOT ${OpenVDB_ROOT}) -elseif(DEFINED ENV{OpenVDB_ROOT}) - set(_OPENVDB_ROOT $ENV{OpenVDB_ROOT}) -elseif(OPENVDB_ROOT) - set(_OPENVDB_ROOT ${OPENVDB_ROOT}) -elseif(DEFINED ENV{OPENVDB_ROOT}) - set(_OPENVDB_ROOT $ENV{OPENVDB_ROOT}) -endif() - -# Additionally try and use pkconfig to find OpenVDB -if(USE_PKGCONFIG) - if(NOT DEFINED PKG_CONFIG_FOUND) - find_package(PkgConfig) - endif() - pkg_check_modules(PC_OpenVDB QUIET OpenVDB) -endif() - -# This CMake module supports being called from external packages AND from -# within the OpenVDB repository for building openvdb components with the -# core library build disabled. Determine where we are being called from: -# -# (repo structure = /cmake/FindOpenVDB.cmake) -# (inst structure = /lib/cmake/OpenVDB/FindOpenVDB.cmake) - -get_filename_component(_DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) - -if(${_DIR_NAME} STREQUAL "cmake") - # Called from root repo for openvdb components -elseif(${_DIR_NAME} STREQUAL "OpenVDB") - # Set the install variable to track directories if this is being called from - # an installed location and from another package. The expected installation - # directory structure is: - # /lib/cmake/OpenVDB/FindOpenVDB.cmake - # /include - # /bin - get_filename_component(_IMPORT_PREFIX ${CMAKE_CURRENT_LIST_DIR} DIRECTORY) - get_filename_component(_IMPORT_PREFIX ${_IMPORT_PREFIX} DIRECTORY) - get_filename_component(_IMPORT_PREFIX ${_IMPORT_PREFIX} DIRECTORY) - set(_OPENVDB_INSTALL ${_IMPORT_PREFIX}) - list(APPEND _OPENVDB_ROOT ${_OPENVDB_INSTALL}) -endif() - -unset(_DIR_NAME) -unset(_IMPORT_PREFIX) - -# ------------------------------------------------------------------------ -# Search for OpenVDB include DIR -# ------------------------------------------------------------------------ - -set(_OPENVDB_INCLUDE_SEARCH_DIRS "") -list(APPEND _OPENVDB_INCLUDE_SEARCH_DIRS - ${OPENVDB_INCLUDEDIR} - ${_OPENVDB_ROOT} - ${PC_OpenVDB_INCLUDE_DIRS} - ${SYSTEM_LIBRARY_PATHS} -) - -foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) - # Add in extra component paths - set(_VDB_COMPONENT_SEARCH_DIRS ${_OPENVDB_INCLUDE_SEARCH_DIRS}) - list(APPEND _VDB_COMPONENT_SEARCH_DIRS - ${OPENVDB_${COMPONENT}_ROOT} - ${OPENVDB_${COMPONENT}_INCLUDEDIR} - ) - if(_VDB_COMPONENT_SEARCH_DIRS) - list(REMOVE_DUPLICATES _VDB_COMPONENT_SEARCH_DIRS) - endif() - - # Look for a standard header files. - if(${COMPONENT} STREQUAL "openvdb") - # Look for a standard OpenVDB header file. - find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR openvdb/version.h - ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} - PATHS ${_VDB_COMPONENT_SEARCH_DIRS} - PATH_SUFFIXES - ${CMAKE_INSTALL_INCLUDEDIR} - include - ) - elseif(${COMPONENT} STREQUAL "pyopenvdb") - find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR pyopenvdb.h - ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} - PATHS ${_VDB_COMPONENT_SEARCH_DIRS} - PATH_SUFFIXES - ${CMAKE_INSTALL_INCLUDEDIR}/openvdb/python - ${CMAKE_INSTALL_INCLUDEDIR}/openvdb - ${CMAKE_INSTALL_INCLUDEDIR} - include - ) - elseif(${COMPONENT} STREQUAL "openvdb_ax") - # Look for a standard OpenVDB header file. - find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR compiler/Compiler.h - ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} - PATHS ${_VDB_COMPONENT_SEARCH_DIRS} - PATH_SUFFIXES - ${CMAKE_INSTALL_INCLUDEDIR}/openvdb/openvdb_ax - ${CMAKE_INSTALL_INCLUDEDIR}/openvdb_ax - ${CMAKE_INSTALL_INCLUDEDIR} - include - ) - elseif(${COMPONENT} STREQUAL "openvdb_houdini") - # @note Expects both houdini_utils and openvdb_houdini folders - # to be located in the same place - find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR openvdb_houdini/SOP_NodeVDB.h - ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} - PATHS ${_VDB_COMPONENT_SEARCH_DIRS} - PATH_SUFFIXES - ${CMAKE_INSTALL_INCLUDEDIR}/openvdb - ${CMAKE_INSTALL_INCLUDEDIR} - include - ) - elseif(${COMPONENT} STREQUAL "nanovdb") - # Look for NanoVDB.h - find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR NanoVDB.h - ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} - PATHS ${_VDB_COMPONENT_SEARCH_DIRS} - PATH_SUFFIXES - ${CMAKE_INSTALL_INCLUDEDIR}/nanovdb - ${CMAKE_INSTALL_INCLUDEDIR} - include - ) - endif() - unset(_VDB_COMPONENT_SEARCH_DIRS) -endforeach() - -set(OpenVDB_INCLUDE_DIR ${OpenVDB_openvdb_INCLUDE_DIR} - CACHE PATH "The OpenVDB core include directory") - -set(_OPENVDB_VERSION_HEADER "${OpenVDB_INCLUDE_DIR}/openvdb/version.h") -OPENVDB_VERSION_FROM_HEADER("${_OPENVDB_VERSION_HEADER}" - VERSION OpenVDB_VERSION - MAJOR OpenVDB_MAJOR_VERSION - MINOR OpenVDB_MINOR_VERSION - PATCH OpenVDB_PATCH_VERSION - ABI OpenVDB_ABI_FROM_HEADER # will be OpenVDB_MAJOR_VERSION prior to 8.1.0 -) - -if(OpenVDB_VERSION VERSION_LESS 8.1.0) - set(_OPENVDB_HAS_NEW_VERSION_HEADER FALSE) - # ABI gets computed later -else() - set(_OPENVDB_HAS_NEW_VERSION_HEADER TRUE) - set(OpenVDB_ABI ${OpenVDB_ABI_FROM_HEADER}) -endif() -unset(OpenVDB_ABI_FROM_HEADER) - -# ------------------------------------------------------------------------ -# Search for OPENVDB lib DIR -# ------------------------------------------------------------------------ - -set(_OPENVDB_LIBRARYDIR_SEARCH_DIRS "") - -# Append to _OPENVDB_LIBRARYDIR_SEARCH_DIRS in priority order - -list(APPEND _OPENVDB_LIBRARYDIR_SEARCH_DIRS - ${OPENVDB_LIBRARYDIR} - ${_OPENVDB_ROOT} - ${PC_OpenVDB_LIBRARY_DIRS} - ${SYSTEM_LIBRARY_PATHS} -) - -# Library suffix handling - -set(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES}) - -set(OPENVDB_PYTHON_PATH_SUFFIXES - lib64/python - lib64/python2.7 - lib64/python3 - lib/python - lib/python2.7 - lib/python3 -) - -# Recurse through all the site-packages and dist-packages on the file system -file(GLOB PYTHON_SITE_PACKAGES ${CMAKE_INSTALL_FULL_LIBDIR}/python**/*) -foreach(_site_package_full_dir ${PYTHON_SITE_PACKAGES}) - string(REPLACE ${CMAKE_INSTALL_FULL_LIBDIR} "${CMAKE_INSTALL_LIBDIR}" - _site_package_dir ${_site_package_full_dir}) - list(APPEND OPENVDB_PYTHON_PATH_SUFFIXES ${_site_package_dir}) -endforeach() - -set(OPENVDB_LIB_PATH_SUFFIXES - ${CMAKE_INSTALL_LIBDIR} - lib64 - lib -) - -list(REMOVE_DUPLICATES OPENVDB_PYTHON_PATH_SUFFIXES) -list(REMOVE_DUPLICATES OPENVDB_LIB_PATH_SUFFIXES) - -# Static library setup -if(WIN32) - if(OPENVDB_USE_STATIC_LIBS) - set(CMAKE_FIND_LIBRARY_SUFFIXES ".lib") - endif() -else() - if(OPENVDB_USE_STATIC_LIBS) - set(CMAKE_FIND_LIBRARY_SUFFIXES ".a") - endif() -endif() - -set(OpenVDB_LIB_COMPONENTS "") - -foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) - message("COMPONENT = " ${COMPONENT}) - set(LIB_NAME ${COMPONENT}) - - # Add in extra component paths - set(_VDB_COMPONENT_SEARCH_DIRS ${_OPENVDB_LIBRARYDIR_SEARCH_DIRS}) - list(APPEND _VDB_COMPONENT_SEARCH_DIRS - ${OPENVDB_${COMPONENT}_ROOT} - ${OPENVDB_${COMPONENT}_LIBRARYDIR} - ) - - if(${COMPONENT} STREQUAL "pyopenvdb") - set(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES}) - set(CMAKE_FIND_LIBRARY_PREFIXES ";lib") # find non-prefixed - find_library(OpenVDB_${COMPONENT}_LIBRARY ${LIB_NAME} - ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} - PATHS ${_VDB_COMPONENT_SEARCH_DIRS} - PATH_SUFFIXES ${OPENVDB_PYTHON_PATH_SUFFIXES} - ) - set(CMAKE_FIND_LIBRARY_PREFIXES ${_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_PREFIXES}) - elseif(${COMPONENT} STREQUAL "openvdb_je") - # alias to the result of openvdb which should be handled first - set(OpenVDB_${COMPONENT}_LIBRARY ${OpenVDB_openvdb_LIBRARY}) - else() - find_library(OpenVDB_${COMPONENT}_LIBRARY ${LIB_NAME} - ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} - PATHS ${_VDB_COMPONENT_SEARCH_DIRS} - PATH_SUFFIXES ${OPENVDB_LIB_PATH_SUFFIXES} - ) - endif() - - list(APPEND OpenVDB_LIB_COMPONENTS ${OpenVDB_${COMPONENT}_LIBRARY}) - if(${COMPONENT} STREQUAL "nanovdb") - # nanovdb is headers-only, no lib component - if(OpenVDB_${COMPONENT}_INCLUDE_DIR) - set(OpenVDB_${COMPONENT}_FOUND TRUE) - else() - set(OpenVDB_${COMPONENT}_FOUND FALSE) - endif() - else() - if(OpenVDB_${COMPONENT}_LIBRARY) - set(OpenVDB_${COMPONENT}_FOUND TRUE) - else() - set(OpenVDB_${COMPONENT}_FOUND FALSE) - endif() - endif() - unset(_VDB_COMPONENT_SEARCH_DIRS) -endforeach() - -unset(OPENVDB_PYTHON_PATH_SUFFIXES) -unset(OPENVDB_LIB_PATH_SUFFIXES) - -# Reset library suffix - -set(CMAKE_FIND_LIBRARY_SUFFIXES ${_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES}) -unset(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES) - -# ------------------------------------------------------------------------ -# Cache and set OPENVDB_FOUND -# ------------------------------------------------------------------------ -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(OpenVDB - FOUND_VAR OpenVDB_FOUND - REQUIRED_VARS - OpenVDB_INCLUDE_DIR - OpenVDB_LIB_COMPONENTS - VERSION_VAR OpenVDB_VERSION - HANDLE_COMPONENTS -) - -# ------------------------------------------------------------------------ -# Determine ABI number -# ------------------------------------------------------------------------ - -# Set the ABI number the library was built against. The old system, -# which didn't define the ABI in the build config, uses vdb_print - -if(NOT _OPENVDB_HAS_NEW_VERSION_HEADER) - if(_OPENVDB_INSTALL) - OPENVDB_ABI_VERSION_FROM_PRINT( - "${_OPENVDB_INSTALL}/bin/vdb_print" - ABI OpenVDB_ABI - ) - else() - # Try and find vdb_print from the include path - OPENVDB_ABI_VERSION_FROM_PRINT( - "${OpenVDB_INCLUDE_DIR}/../bin/vdb_print" - ABI OpenVDB_ABI - ) - endif() -endif() - -if(NOT OpenVDB_FIND_QUIET) - if(NOT OpenVDB_ABI) - message(WARNING "Unable to determine OpenVDB ABI version from OpenVDB " - "installation. The library major version \"${OpenVDB_MAJOR_VERSION}\" " - "will be inferred. If this is not correct, use " - "add_definitions(-DOPENVDB_ABI_VERSION_NUMBER=N)" - ) - else() - message(STATUS "OpenVDB ABI Version: ${OpenVDB_ABI}") - endif() -endif() - -# ------------------------------------------------------------------------ -# Handle OpenVDB dependencies and interface settings -# ------------------------------------------------------------------------ - -# Handle openvdb_houdini first to configure search paths - -if(openvdb_houdini IN_LIST OpenVDB_FIND_COMPONENTS) - include(OpenVDBHoudiniSetup) -endif() - -# Add standard dependencies - -find_package(TBB REQUIRED COMPONENTS tbb) - -if(NOT OPENVDB_USE_STATIC_LIBS AND NOT Boost_USE_STATIC_LIBS) - # @note Both of these must be set for Boost 1.70 (VFX2020) to link against - # boost shared libraries (more specifically libraries built with -fPIC). - # http://boost.2283326.n4.nabble.com/CMake-config-scripts-broken-in-1-70-td4708957.html - # https://github.com/boostorg/boost_install/commit/160c7cb2b2c720e74463865ef0454d4c4cd9ae7c - set(BUILD_SHARED_LIBS ON) - set(Boost_USE_STATIC_LIBS OFF) -endif() - -find_package(Boost REQUIRED COMPONENTS iostreams system) - -# Add deps for pyopenvdb -# @todo track for numpy - -if(pyopenvdb IN_LIST OpenVDB_FIND_COMPONENTS) - find_package(PythonLibs REQUIRED) - - # Boost python handling - try and find both python and pythonXx (version suffixed). - # Prioritize the version suffixed library, failing if neither exist. - - find_package(Boost ${MINIMUM_BOOST_VERSION} - QUIET COMPONENTS python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR} - ) - - if(TARGET Boost::python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) - set(BOOST_PYTHON_LIB "python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}") - message(STATUS "Found boost_python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}") - else() - find_package(Boost ${MINIMUM_BOOST_VERSION} QUIET COMPONENTS python) - if(TARGET Boost::python) - set(BOOST_PYTHON_LIB "python") - message(STATUS "Found non-suffixed boost_python, assuming to be python version " - "\"${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}\" compatible" - ) - else() - message(FATAL_ERROR "Unable to find boost_python or " - "boost_python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}." - ) - endif() - endif() -endif() - -# Add deps for openvdb_ax - -if(openvdb_ax IN_LIST OpenVDB_FIND_COMPONENTS) - find_package(LLVM REQUIRED) - find_library(found_LLVM LLVM HINTS ${LLVM_LIBRARY_DIRS}) - - if(found_LLVM) - set(LLVM_LIBS "LLVM") - else() - llvm_map_components_to_libnames(_llvm_libs - native core executionengine support mcjit passes objcarcopts) - set(LLVM_LIBS "${_llvm_libs}") - endif() - - if(NOT OpenVDB_FIND_QUIET) - message(STATUS "Found LLVM: ${LLVM_DIR} (found version \"${LLVM_PACKAGE_VERSION}\")") - endif() -endif() - -# As the way we resolve optional libraries relies on library file names, use -# the configuration options from the main CMakeLists.txt to allow users -# to manually identify the requirements of OpenVDB builds if they know them. -set(OpenVDB_USES_BLOSC ${USE_BLOSC}) -set(OpenVDB_USES_ZLIB ${USE_ZLIB}) -set(OpenVDB_USES_LOG4CPLUS ${USE_LOG4CPLUS}) -set(OpenVDB_USES_IMATH_HALF ${USE_IMATH_HALF}) -set(OpenVDB_DEFINITIONS) - -if(WIN32) - if(OPENVDB_USE_STATIC_LIBS) - list(APPEND OpenVDB_DEFINITIONS OPENVDB_STATICLIB) - else() - list(APPEND OpenVDB_DEFINITIONS OPENVDB_DLL) - endif() - # Newer version of OpenVDB define these in Platform.h, but they are also - # provided here to maintain backwards compatibility with header include - # others - list(APPEND OpenVDB_DEFINITIONS _WIN32) - list(APPEND OpenVDB_DEFINITIONS NOMINMAX) -endif() - -if(OpenVDB_ABI) - # Newer version of OpenVDB defines this in version.h, but it is are also - # provided here to maintain backwards compatibility with header include - # others - list(APPEND OpenVDB_DEFINITIONS OPENVDB_ABI_VERSION_NUMBER=${OpenVDB_ABI}) -endif() - -# Configure deps - -if(_OPENVDB_HAS_NEW_VERSION_HEADER) - OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_IMATH_HALF" OpenVDB_USES_IMATH_HALF) - OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_BLOSC" OpenVDB_USES_BLOSC) - OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_ZLIB" OpenVDB_USES_ZLIB) -elseif(NOT OPENVDB_USE_STATIC_LIBS) - # Use GetPrerequisites to see which libraries this OpenVDB lib has linked to - # which we can query for optional deps. This basically runs ldd/otoll/objdump - # etc to track deps. We could use a vdb_config binary tools here to improve - # this process - include(GetPrerequisites) - - set(_EXCLUDE_SYSTEM_PREREQUISITES 1) - set(_RECURSE_PREREQUISITES 0) - set(_OPENVDB_PREREQUISITE_LIST) - - get_prerequisites(${OpenVDB_openvdb_LIBRARY} - _OPENVDB_PREREQUISITE_LIST - ${_EXCLUDE_SYSTEM_PREREQUISITES} - ${_RECURSE_PREREQUISITES} - "" - "${SYSTEM_LIBRARY_PATHS}" - ) - - unset(_EXCLUDE_SYSTEM_PREREQUISITES) - unset(_RECURSE_PREREQUISITES) - - # Search for optional dependencies - foreach(PREREQUISITE ${_OPENVDB_PREREQUISITE_LIST}) - set(_HAS_DEP) - get_filename_component(PREREQUISITE ${PREREQUISITE} NAME) - - string(FIND ${PREREQUISITE} "blosc" _HAS_DEP) - if(NOT ${_HAS_DEP} EQUAL -1) - set(OpenVDB_USES_BLOSC ON) - endif() - - string(FIND ${PREREQUISITE} "zlib" _HAS_DEP) - if(NOT ${_HAS_DEP} EQUAL -1) - set(OpenVDB_USES_ZLIB ON) - endif() - - string(FIND ${PREREQUISITE} "log4cplus" _HAS_DEP) - if(NOT ${_HAS_DEP} EQUAL -1) - set(OpenVDB_USES_LOG4CPLUS ON) - endif() - - string(FIND ${PREREQUISITE} "Half" _HAS_DEP) - if(NOT ${_HAS_DEP} EQUAL -1) - set(OpenVDB_USES_IMATH_HALF ON) - endif() - endforeach() - - unset(_OPENVDB_PREREQUISITE_LIST) -endif() - -if(OpenVDB_USES_BLOSC) - find_package(Blosc REQUIRED) -endif() - -if(OpenVDB_USES_ZLIB) - find_package(ZLIB REQUIRED) -endif() - -if(OpenVDB_USES_LOG4CPLUS) - find_package(Log4cplus REQUIRED) -endif() - -if(OpenVDB_USES_IMATH_HALF) - find_package(Imath CONFIG) - if (NOT TARGET Imath::Imath) - find_package(IlmBase REQUIRED COMPONENTS Half) - endif() - - if(WIN32) - # @note OPENVDB_OPENEXR_STATICLIB is old functionality and should be removed - if(OPENEXR_USE_STATIC_LIBS OR - (${ILMBASE_LIB_TYPE} STREQUAL STATIC_LIBRARY) OR - (${IMATH_LIB_TYPE} STREQUAL STATIC_LIBRARY)) - list(APPEND OpenVDB_DEFINITIONS OPENVDB_OPENEXR_STATICLIB) - endif() - endif() -endif() - -if(UNIX) - find_package(Threads REQUIRED) -endif() - -# Set deps. Note that the order here is important. If we're building against -# Houdini 17.5 we must include IlmBase deps first to ensure the users chosen -# namespaced headers are correctly prioritized. Otherwise other include paths -# from shared installs (including houdini) may pull in the wrong headers - -set(_OPENVDB_VISIBLE_DEPENDENCIES - Boost::iostreams - Boost::system -) - -if(OpenVDB_USES_IMATH_HALF) - list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES $ $) -endif() - -if(OpenVDB_USES_LOG4CPLUS) - list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES Log4cplus::log4cplus) - list(APPEND OpenVDB_DEFINITIONS OPENVDB_USE_LOG4CPLUS) -endif() - -list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES - TBB::tbb -) -if(UNIX) - list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES - Threads::Threads - ) -endif() - -set(_OPENVDB_HIDDEN_DEPENDENCIES) - -if(NOT OPENVDB_USE_STATIC_LIBS) - if(OpenVDB_USES_BLOSC) - list(APPEND _OPENVDB_HIDDEN_DEPENDENCIES Blosc::blosc) - endif() - if(OpenVDB_USES_ZLIB) - list(APPEND _OPENVDB_HIDDEN_DEPENDENCIES ZLIB::ZLIB) - endif() -endif() - -if(openvdb_je IN_LIST OpenVDB_FIND_COMPONENTS) - find_package(Jemalloc REQUIRED) -endif() - -# ------------------------------------------------------------------------ -# Configure imported targets -# ------------------------------------------------------------------------ - -set(OpenVDB_LIBRARIES ${OpenVDB_LIB_COMPONENTS}) -set(OpenVDB_INCLUDE_DIRS ${OpenVDB_INCLUDE_DIR}) - -set(OpenVDB_LIBRARY_DIRS "") -foreach(LIB ${OpenVDB_LIB_COMPONENTS}) - get_filename_component(_OPENVDB_LIBDIR ${LIB} DIRECTORY) - list(APPEND OpenVDB_LIBRARY_DIRS ${_OPENVDB_LIBDIR}) -endforeach() -list(REMOVE_DUPLICATES OpenVDB_LIBRARY_DIRS) - -# OpenVDB::openvdb - -if(NOT TARGET OpenVDB::openvdb) - set(OPENVDB_openvdb_LIB_TYPE UNKNOWN) - if(OPENVDB_USE_STATIC_LIBS) - set(OPENVDB_openvdb_LIB_TYPE STATIC) - elseif(UNIX) - get_filename_component(_OPENVDB_openvdb_EXT - ${OpenVDB_openvdb_LIBRARY} EXT) - if(_OPENVDB_openvdb_EXT STREQUAL ".a") - set(OPENVDB_openvdb_LIB_TYPE STATIC) - elseif(_OPENVDB_openvdb_EXT STREQUAL ".so" OR - _OPENVDB_openvdb_EXT STREQUAL ".dylib") - set(OPENVDB_openvdb_LIB_TYPE SHARED) - endif() - endif() - - add_library(OpenVDB::openvdb ${OPENVDB_openvdb_LIB_TYPE} IMPORTED) - set_target_properties(OpenVDB::openvdb PROPERTIES - IMPORTED_LOCATION "${OpenVDB_openvdb_LIBRARY}" - INTERFACE_COMPILE_OPTIONS "${PC_OpenVDB_CFLAGS_OTHER}" - INTERFACE_COMPILE_DEFINITIONS "${OpenVDB_DEFINITIONS}" - INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_INCLUDE_DIR}" - IMPORTED_LINK_DEPENDENT_LIBRARIES "${_OPENVDB_HIDDEN_DEPENDENCIES}" # non visible deps - INTERFACE_LINK_LIBRARIES "${_OPENVDB_VISIBLE_DEPENDENCIES}" # visible deps (headers) - INTERFACE_COMPILE_FEATURES cxx_std_14 - ) -endif() - -# OpenVDB::openvdb_je - -if(OpenVDB_openvdb_je_LIBRARY) - if(NOT TARGET OpenVDB::openvdb_je) - add_library(OpenVDB::openvdb_je INTERFACE IMPORTED) - target_link_libraries(OpenVDB::openvdb_je INTERFACE OpenVDB::openvdb) - target_link_libraries(OpenVDB::openvdb_je INTERFACE Jemalloc::jemalloc) - endif() -endif() - -# OpenVDB::pyopenvdb - -if(OpenVDB_pyopenvdb_LIBRARY) - if(NOT TARGET OpenVDB::pyopenvdb) - add_library(OpenVDB::pyopenvdb MODULE IMPORTED) - set_target_properties(OpenVDB::pyopenvdb PROPERTIES - IMPORTED_LOCATION "${OpenVDB_pyopenvdb_LIBRARY}" - INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_pyopenvdb_INCLUDE_DIR};${PYTHON_INCLUDE_DIR}" - INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;Boost::${BOOST_PYTHON_LIB};${PYTHON_LIBRARIES}" - INTERFACE_COMPILE_FEATURES cxx_std_14 - ) - endif() -endif() - -# OpenVDB::openvdb_houdini - -if(OpenVDB_openvdb_houdini_LIBRARY) - if(NOT TARGET OpenVDB::openvdb_houdini) - add_library(OpenVDB::openvdb_houdini SHARED IMPORTED) - set_target_properties(OpenVDB::openvdb_houdini PROPERTIES - IMPORTED_LOCATION "${OpenVDB_openvdb_houdini_LIBRARY}" - INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_openvdb_houdini_INCLUDE_DIR}" - INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;Houdini" - INTERFACE_COMPILE_FEATURES cxx_std_14 - ) - endif() -endif() - -# OpenVDB::openvdb_ax - -if(OpenVDB_openvdb_ax_LIBRARY) - set(OPENVDB_openvdb_ax_LIB_TYPE UNKNOWN) - if(OPENVDB_USE_STATIC_LIBS) - set(OPENVDB_openvdb_ax_LIB_TYPE STATIC) - elseif(UNIX) - get_filename_component(_OPENVDB_openvdb_ax_EXT - ${OpenVDB_openvdb_ax_LIBRARY} EXT) - if(_OPENVDB_openvdb_ax_EXT STREQUAL ".a") - set(OPENVDB_openvdb_ax_LIB_TYPE STATIC) - elseif(_OPENVDB_openvdb_ax_EXT STREQUAL ".so" OR - _OPENVDB_openvdb_ax_EXT STREQUAL ".dylib") - set(OPENVDB_openvdb_ax_LIB_TYPE SHARED) - endif() - endif() - - - if(NOT TARGET OpenVDB::openvdb_ax) - add_library(OpenVDB::openvdb_ax UNKNOWN IMPORTED) - set_target_properties(OpenVDB::openvdb_ax PROPERTIES - IMPORTED_LOCATION "${OpenVDB_openvdb_ax_LIBRARY}" - INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_openvdb_ax_INCLUDE_DIR}" - INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${LLVM_INCLUDE_DIRS}" - INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;${LLVM_LIBS}" - INTERFACE_COMPILE_FEATURES cxx_std_14 - ) - endif() -endif() - -# OpenVDB::nanovdb - -if(OpenVDB_nanovdb_LIBRARY) - if(NOT TARGET OpenVDB::nanovdb) - add_library(OpenVDB::nanovdb INTERFACE IMPORTED) - set_target_properties(OpenVDB::nanovdb PROPERTIES - IMPORTED_LOCATION "${OpenVDB_nanovdb_LIBRARY}" - INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_nanovdb_INCLUDE_DIR}" - INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;" - INTERFACE_COMPILE_FEATURES cxx_std_14 - ) - endif() -endif() - -unset(_OPENVDB_VISIBLE_DEPENDENCIES) -unset(_OPENVDB_HIDDEN_DEPENDENCIES) diff --git a/robot/ros_ws/src/global/planners/exploration/CMakeModules/OpenVDBUtils.cmake b/robot/ros_ws/src/global/planners/exploration/CMakeModules/OpenVDBUtils.cmake deleted file mode 100644 index 084f27d71..000000000 --- a/robot/ros_ws/src/global/planners/exploration/CMakeModules/OpenVDBUtils.cmake +++ /dev/null @@ -1,164 +0,0 @@ -# Copyright Contributors to the OpenVDB Project -# SPDX-License-Identifier: MPL-2.0 -# -#[=======================================================================[.rst: - -OpenVDBUtils.cmake ------------------- - -A utility CMake file which provides helper functions for configuring an -OpenVDB installation. - -Use this module by invoking include with the form:: - - include ( OpenVDBUtils ) - - -The following functions are provided: - -``OPENVDB_GET_VERSION_DEFINE`` - - OPENVDB_GET_VERSION_DEFINE ( ) - - Parse the provided version file to retrieve the a given OpenVDB define - specified by and store the result in . If the define has a - value, the value is stored. If the define has no value but exists, ON is - stored. Else, OFF is stored. - - If the file does not exist, is unmodified. - -``OPENVDB_VERSION_FROM_HEADER`` - - OPENVDB_VERSION_FROM_HEADER ( - VERSION [] - MAJOR [] - MINOR [] - PATCH [] ) - - Parse the provided version file to retrieve the current OpenVDB - version information. The file is expected to be a version.h file - as found in the following path of an OpenVDB repository: - openvdb/version.h - - If the file does not exist, variables are unmodified. - -``OPENVDB_ABI_VERSION_FROM_PRINT`` - - OPENVDB_ABI_VERSION_FROM_PRINT ( - [QUIET] - ABI [] ) - - Retrieve the ABI version that an installation of OpenVDB was compiled - for using the provided vdb_print binary. Parses the result of: - vdb_print --version - - If the binary does not exist or fails to launch, variables are - unmodified. - -#]=======================================================================] - -cmake_minimum_required(VERSION 3.15) - - -function(OPENVDB_GET_VERSION_DEFINE HEADER KEY VALUE) - if(NOT EXISTS ${HEADER}) - return() - endif() - - file(STRINGS "${HEADER}" details REGEX "^#define[\t ]+${KEY}[\t ]+.*") - if(details) - # define has a value, extract it and return - string(REGEX REPLACE "^.*${KEY}[\t ]+([0-9]*).*$" "\\1" details "${details}") - set(${VALUE} ${details} PARENT_SCOPE) - return() - endif() - - # try re-searching for single defines - file(STRINGS "${HEADER}" details REGEX "^#define[\t ]+${KEY}.*") - if(details) - set(${VALUE} ON PARENT_SCOPE) - else() - set(${VALUE} OFF PARENT_SCOPE) - endif() -endfunction() - - -######################################################################## -######################################################################## - - -function(OPENVDB_VERSION_FROM_HEADER OPENVDB_VERSION_FILE) - cmake_parse_arguments(_VDB "" "VERSION;MAJOR;MINOR;PATCH;ABI" "" ${ARGN}) - - if(NOT EXISTS ${OPENVDB_VERSION_FILE}) - return() - endif() - - OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_MAJOR_VERSION_NUMBER" _OpenVDB_MAJOR_VERSION) - OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_MINOR_VERSION_NUMBER" _OpenVDB_MINOR_VERSION) - OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_PATCH_VERSION_NUMBER" _OpenVDB_PATCH_VERSION) - OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_ABI_VERSION_NUMBER" _OpenVDB_ABI_VERSION) - - if(_VDB_VERSION) - set(${_VDB_VERSION} - ${_OpenVDB_MAJOR_VERSION}.${_OpenVDB_MINOR_VERSION}.${_OpenVDB_PATCH_VERSION} - PARENT_SCOPE - ) - endif() - if(_VDB_MAJOR) - set(${_VDB_MAJOR} ${_OpenVDB_MAJOR_VERSION} PARENT_SCOPE) - endif() - if(_VDB_MINOR) - set(${_VDB_MINOR} ${_OpenVDB_MINOR_VERSION} PARENT_SCOPE) - endif() - if(_VDB_PATCH) - set(${_VDB_PATCH} ${_OpenVDB_PATCH_VERSION} PARENT_SCOPE) - endif() - if(_VDB_ABI) - set(${_VDB_ABI} ${_OpenVDB_ABI_VERSION} PARENT_SCOPE) - endif() -endfunction() - - -######################################################################## -######################################################################## - - -function(OPENVDB_ABI_VERSION_FROM_PRINT OPENVDB_PRINT) - cmake_parse_arguments(_VDB "QUIET" "ABI" "" ${ARGN}) - - if(NOT EXISTS ${OPENVDB_PRINT}) - return() - endif() - - set(_VDB_PRINT_VERSION_STRING "") - set(_VDB_PRINT_RETURN_STATUS "") - - if(${_VDB_QUIET}) - execute_process(COMMAND ${OPENVDB_PRINT} "--version" - RESULT_VARIABLE _VDB_PRINT_RETURN_STATUS - OUTPUT_VARIABLE _VDB_PRINT_VERSION_STRING - ERROR_QUIET - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - else() - execute_process(COMMAND ${OPENVDB_PRINT} "--version" - RESULT_VARIABLE _VDB_PRINT_RETURN_STATUS - OUTPUT_VARIABLE _VDB_PRINT_VERSION_STRING - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - endif() - - if(${_VDB_PRINT_RETURN_STATUS}) - return() - endif() - - set(_OpenVDB_ABI) - string(REGEX REPLACE ".*abi([0-9]*).*" "\\1" _OpenVDB_ABI ${_VDB_PRINT_VERSION_STRING}) - unset(_VDB_PRINT_RETURN_STATUS) - unset(_VDB_PRINT_VERSION_STRING) - - if(_VDB_ABI) - set(${_VDB_ABI} ${_OpenVDB_ABI} PARENT_SCOPE) - endif() -endfunction() diff --git a/robot/ros_ws/src/global/planners/exploration/README.md b/robot/ros_ws/src/global/planners/exploration/README.md deleted file mode 100644 index 0e609baa8..000000000 --- a/robot/ros_ws/src/global/planners/exploration/README.md +++ /dev/null @@ -1,55 +0,0 @@ -# Exploration Planner - -The Exploration Planner is an optional global planner for autonomous flight. It maintains an OpenVDB-based voxel occupancy grid map, extracts frontiers from the map, and generates trajectories that let the drone explore unvisited areas. Because it maintains its own map, you can turn off the `vdb_mapping` node in the launch XML; the planner will generate and publish multiple linked straight-line trajectories as a shortened, collision-checked RRT path to a selected viewpoint. - -## Functionality - -In your stack's entry launch file (e.g. a copy of -`stacks/full_default/launch/stack.launch.xml`), replace the -`random_walk_planner.launch.xml` include (and optionally the vdb_mapping -include) with: - -``: - -Then when running the robot stack, after taking off, toggle the global plan from the rviz Tasks Panel or the GCS (the `~/global_plan_toggle` service) and the exploration planner runs in place of the random walk planner. The working process is: - -1. Create and maintain a voxel grid map with odometry and laser scan, to visualize, check topic `"~/vdb_viz"`, where `~` is the namespace. -2. Extract frontier and select viewpoints for exploration. -3. Generate RRT path, shorten and publish as global plan. -4. Continuously monitor the robot's progress along the published path. -5. Once the robot completes the current path, a new exploration path will be generated. - -This loop continues, enabling the drone to keep exploring the entire space. - -The planner is derived from the random walk planner and shares several of its parameters; parameter documentation is still being expanded. - -## Parameters -|
Parameter
| Description -|----------------------------|--------------------------------------------------------------- -| `num_paths_to_generate` | Number of straight-line paths to concatenate into a complete trajectory.| -| `max_start_to_goal_dist_m` | Maximum distance (in meters) from the start point to the goal point for each straight-line segment.| -| `checking_point_cnt` | Number of points along each straight-line segment to check for collisions.| -| `max_z_change_m` | Maximum allowed change in height (z-axis) between the start and goal points.| -| `collision_padding_m` | Extra padding (in meters) added around a voxel's dimensions when checking for collisions.| -| `path_end_threshold_m` | Distance threshold (in meters) for considering the current path completed and generating a new one.| -| `max_yaw_change_degrees` | Maximum allowed change in angle (in radians) between consecutive straight-line segments to ensure a relatively consistent direction.| -| `robot_frame_id` | The frame name for the robot's base frame to look up the transform from the robot position to the world.| - -## Services -|
Parameter
| Type | Description -|----------------------------|----------------------------------------|-----------------------| -| `~/global_plan_toggle` | std_srvs/Trigger | A toggle switch to turn on and off the exploration planner.| - -## Subscriptions -|
Parameter
| Type | Description -|----------------------------|----------------------------------------|-----------------------| -| `~/sub_map_topic` | visualization_msgs/Marker | Stores the map representation that is output from the world or local map topic; currently using vdb local map.| -| `~/tf` | geometry_msgs/TransformStamped | Stores the transform from the robot to the world.| - -## Publications -|
Parameter
| Type | Description -|----------------------------|----------------------------------------|-----------------------| -| `~/pub_global_plan_topic` | nav_msgs/Path | Outputs the global plan generated by the exploration planner.| - - - diff --git a/robot/ros_ws/src/global/planners/exploration/config/exploration_config.yaml b/robot/ros_ws/src/global/planners/exploration/config/exploration_config.yaml deleted file mode 100644 index 4b3bbb0b7..000000000 --- a/robot/ros_ws/src/global/planners/exploration/config/exploration_config.yaml +++ /dev/null @@ -1,91 +0,0 @@ -/**: - ros__parameters: - robot_frame_id: "base_link" - lidar_frame_id: "ouster" - map_frame_id: "map" - pub_global_plan_topic: "~/global_plan" - pub_goal_point_viz_topic: "~/goal_point_viz" - pub_trajectory_viz_topic: "~/traj_viz" - pub_grid_viz_topic: "~/vdb_viz" - pub_frontier_viz_topic: "~/frontier_viz" - pub_clustered_frontier_viz_topic: "~/clustered_frontier_viz" - sub_lidar_topic: "sensor_pointcloud" - sub_map_topic: "vdb_map_visualization" - sub_vdb_topic: "vdb_grid" - sub_robot_tf_topic: "/tf" - sub_target_path_topic: "/perspective_goals" - srv_exploration_toggle_topic: "~/global_plan_toggle" - - publish_visualizations: false # should trajectory visualizations be published - num_paths_to_generate: 5 # how many paths to string together - - # Planner Parameters - max_start_to_goal_dist_m: 10.0 # how max distance the goal can be from the current robot position - checking_point_cnt: 100 # how many points to collision check between the start and goal points - max_z_change_m: 2.0 # max height that the goal can deviate from the current height - collision_padding_m: 0.5 # how much space should the path have to any obstacles - path_end_threshold_m: 0.2 # how close to the goal will the path be considered complete - max_yaw_change_degrees: 180.0 # how much the goal can deviate from the current orientation - - # Mapping Parameters - map_voxel_resolution: 0.25 # voxel grid size - l_occ: 1.0 - cluster_cube_dim: 5.0 - voxel_cluster_count_thresh: 50 - - # Stall detection parameters - position_change_threshold: 0.2 # Minimum distance (meters) to consider as movement - stall_timeout_seconds: 20.0 # Time without movement before clearing plan - - # view sampling - bbox_length : 1.5 - bbox_breadth : 1.5 - bbox_height : 1.5 - viewing_distance : 1.5 # not used - num_viewpts_per_cluster : 6 - kViewingDistanceInnerR : 1.5 - kViewingDistanceOuterR : 2.0 - kViewingDistanceZOffset : 1.0 - viewp_bbox_unknown_frac_thresh : 1.0 # In a box around a voxel, all unknown as free, all are allowed to be unknown - viewp_bbox_known_occ_thresh : 0.0 # No occupation allowed - coll_check_endpoint_offset : 0.4 - robot_model_bbox_fraction : 0.1 - - # Traj Planning - too_close_distance: 4.0 - too_close_penalty: 5.0 - odometry_match_distance: 3.0 - odometry_match_angle: 3.14159 - odometry_match_penalty: 1000.0 - momentum_collision_check_distance: 15.0 - momentum_collision_check_distance_min: 4.0 - momentum_change_max_reward: 6.0 - momentum_change_collision_reward: 2.0 - viewpoint_neighborhood_too_close_radius: 10.0 - viewpoint_neighborhood_sphere_radius: 6.0 - viewpoint_neighborhood_count: 5 - momentum_minimum_distance: 0.5 - momentum_time: 6.0 - momentum_collision_check_step_size: 0.4 - - # Voxxel Planner RRT params - planner_norm_limit: 1.0 - dense_step: 0.1 - max_explore_dist: 100.0 - planner_max_iter: 20000 - traj_smooth_horizon: 50 - max_connection_iter: 1000 - - # RRT force adding nodes around the start and goal - rrt_region_nodes_count: 16 - rrt_region_nodes_radius: 0.4 - rrt_region_nodes_z_layers_count_up: 1 - rrt_region_nodes_z_layers_step: 0.4 - - priority_level_thred: 10 - - # Collision checker (Safe) - robot_model_safer_cube_dim: 1.8 - - # Collision checker (Risky) - robot_model_cube_dim: 1.4 #0.8 \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/exploration_vis_path.png b/robot/ros_ws/src/global/planners/exploration/exploration_vis_path.png deleted file mode 100644 index 029e9323f..000000000 Binary files a/robot/ros_ws/src/global/planners/exploration/exploration_vis_path.png and /dev/null differ diff --git a/robot/ros_ws/src/global/planners/exploration/exploration_vis_vdb.png b/robot/ros_ws/src/global/planners/exploration/exploration_vis_vdb.png deleted file mode 100644 index 023090bb8..000000000 Binary files a/robot/ros_ws/src/global/planners/exploration/exploration_vis_vdb.png and /dev/null differ diff --git a/robot/ros_ws/src/global/planners/exploration/include/exploration_logic.hpp b/robot/ros_ws/src/global/planners/exploration/include/exploration_logic.hpp deleted file mode 100644 index 962357d93..000000000 --- a/robot/ros_ws/src/global/planners/exploration/include/exploration_logic.hpp +++ /dev/null @@ -1,222 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "utils/utils.hpp" -#include "utils/rrt_planner.h" -#include "viewpoint_sampling.hpp" -#include "utils/viewpoint.hpp" -#include "utils/collision_checker.h" -#include "rclcpp/rclcpp.hpp" - -typedef std::vector> Path; // x, y, z, yaw -using GridT = openvdb::Grid::Type>; - -struct TimedViewPoint -{ - ViewPoint point; - rclcpp::Time time; -}; - -struct init_params -{ - float max_start_to_goal_dist_m; - int checking_point_cnt; - float max_z_change_m; - float collision_padding_m; - float path_end_threshold_m; - float max_yaw_change_degrees; - - std::tuple voxel_size_m; - - // viewpoint sampling params - double kLOcc_; - double kVoxSize_; - double kViewingDistance_; - double kBboxLength_, kBboxBreadth_, kBboxHeight_; - int kNumViewpointsPerCluster_; - double kViewingDistanceInnerR_, kViewingDistanceOuterR_, kViewingDistanceZOffset_; - double kCollCheckEndpointOffset_; - double kRobotModelBBoxFraction_; - double kViewpBBoxUnknownFracThresh_, kViewpBBoxOccupiedFracThresh_; - - // in init_params - double too_close_distance, too_close_penalty; - double odometry_match_distance, odometry_match_angle, odometry_match_penalty; - double momentum_collision_check_distance, momentum_collision_check_distance_min; - double momentum_change_max_reward, momentum_change_collision_reward; - double viewpoint_neighborhood_too_close_radius, viewpoint_neighborhood_sphere_radius; - int viewpoint_neighborhood_count; - double momentum_minimum_distance; - double momentum_time; - double momentum_collision_check_step_size; - - // RRT - double planner_norm_limit_; - double max_explore_dist_; - int planner_max_iter_; - int max_connection_iter_; - int traj_smooth_horizon_; - int rrt_region_nodes_count_; - double rrt_region_nodes_radius_; - int rrt_region_nodes_z_layers_count_up_; - double rrt_region_nodes_z_layers_step_; - - int priority_level_thred_; - double dense_step_; -}; - -class ExplorationPlanner -{ -public: - ExplorationPlanner() = default; - ExplorationPlanner(init_params params); - ~ExplorationPlanner() = default; - - std::optional generate_straight_rand_path( - std::tuple start_point, - float timeout_duration); // x, y, z, yaw - - std::optional select_viewpoint_and_plan(const ViewPoint& start_point, float timeout_duration); - - std::optional plan_to_given_waypoint(const ViewPoint& start_point, const ViewPoint& goal_point); - - float path_end_threshold_m; - - std::vector> voxel_points; - - bool first_grid_received = false; - bool grid_ready = false; - - GridT::Ptr vdb_grid; - - openvdb::FloatGrid::Ptr occ_grid = openvdb::FloatGrid::create(0.0); - openvdb::FloatGrid::Ptr prev_grid = openvdb::FloatGrid::create(0.0); // to save time for frontier extraction, only check diff for new free voxels - openvdb::BoolGrid::Ptr frontier_grid = openvdb::BoolGrid::create(false); - - std::vector> clustered_points_; - - std::shared_ptr viewp_sample_; - - collision_checker_ns::CollisionChecker collision_checker_; - - std::vector executed_trajectory_; - std::vector momentum_executed_trajectory_; - - RRT_Planner rrt_planner_; - double dense_step_; - -private: - // Numerical constants - float max_start_to_goal_dist_m_; - int checking_point_cnt; - float max_z_change_m_; - float collision_padding_m; - float max_yaw_change_degrees; - - double kLOcc_; - double kVoxSize_; - double kViewingDistance_; - double kBboxLength_, kBboxBreadth_, kBboxHeight_; - int kNumViewpointsPerCluster_; - double kViewingDistanceInnerR_, kViewingDistanceOuterR_, kViewingDistanceZOffset_; - double kCollCheckEndpointOffset_; - double kRobotModelBBoxFraction_; - double kViewpBBoxUnknownFracThresh_, kViewpBBoxOccupiedFracThresh_; - - double too_close_distance_, too_close_penalty_; - double odometry_match_distance_, odometry_match_angle_, odometry_match_penalty_; - double momentum_collision_check_distance_, momentum_collision_check_distance_min_; - double momentum_change_max_reward_, momentum_change_collision_reward_; - double viewpoint_neighborhood_too_close_radius_, viewpoint_neighborhood_sphere_radius_; - int viewpoint_neighborhood_count_; - double momentum_minimum_distance_; - double momentum_time_; - double momentum_collision_check_step_size_; - - double planner_norm_limit_; - double max_explore_dist_; - int planner_max_iter_; - int max_connection_iter_; - int traj_smooth_horizon_; - int rrt_region_nodes_count_; - double rrt_region_nodes_radius_; - int rrt_region_nodes_z_layers_count_up_; - double rrt_region_nodes_z_layers_step_; - - int priority_level_thred_; - - ViewPoint robot_pos_, explore_goal_; - - bool explore_goal_cleared_by_other_robot_; - - // Variables - std::tuple voxel_size_m; - - std::mutex mutex; - - std::vector sampled_viewpoints; - - // Functions - bool check_if_collided_single_voxel(const std::tuple &point, - const std::tuple &voxel_center); - - bool check_if_collided(const std::tuple &point); - - double angleBetweenHeadingAndMomentum(const ViewPoint &point); - - bool computeMomentumVector(double &momentum_direction_x, double &momentum_direction_y, double &momentum_direction_z); - - float getCylinderReward(const int &viewpoint_index, const ViewPoint &planning_point); - - bool extractCylinderViewpointDistances(const int &viewpoint_index, const ViewPoint &planning_point, std::vector &viewpoint_distances); - - bool doesPointMatchTrajectory(const ViewPoint &point); - - void goalSelector(int priority_level); - - static bool compareFrontier(const ViewPoint &p1, const ViewPoint &p2); - - std::tuple generate_goal_point(std::tuple start_point); -}; - -double get_point_distance(const std::tuple &point1, - const std::tuple &point2); - -double deg2rad(double deg); - -double rad2deg(double rad); - -bool lineSegDistances(const ViewPoint &point, - const ViewPoint &line_segment_start, - const ViewPoint &line_segment_end, - double ¶llel_distance, - double &perpendicular_distance); \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/include/exploration_node.hpp b/robot/ros_ws/src/global/planners/exploration/include/exploration_node.hpp deleted file mode 100644 index 7b1f0c16e..000000000 --- a/robot/ros_ws/src/global/planners/exploration/include/exploration_node.hpp +++ /dev/null @@ -1,158 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "exploration_logic.hpp" -#include "utils/utils.hpp" -#include "rclcpp/rclcpp.hpp" -#include - -class ExplorationNode : public rclcpp::Node -{ -protected: - // Planner - // ExplorationPlanner exploration_planner; - std::unique_ptr exploration_planner; - - // String constants - std::string world_frame_id_; - std::string robot_frame_id_; - std::string lidar_frame_id_; - std::string map_frame_id_; - std::string pub_global_plan_topic_; - std::string pub_goal_point_viz_topic_; - std::string pub_trajectory_viz_topic_; - std::string pub_grid_viz_topic_; - std::string pub_frontier_viz_topic_; - std::string pub_clustered_frontier_viz_topic_; - std::string sub_lidar_topic_; - std::string sub_map_topic_; - std::string sub_vdb_topic_; - std::string sub_robot_tf_topic_; - std::string srv_exploration_toggle_topic_; - - // Variables - init_params params; - // nav_msgs::msg::Path generated_path; - double momentum_time_; - int num_paths_to_generate_; - std::vector generated_paths; - bool publish_visualizations = false; - bool received_first_map = false; - bool received_first_robot_tf = false; - bool enable_exploration = false; - bool is_path_executing = false; - - geometry_msgs::msg::Transform current_location; // x, y, z, yaw - geometry_msgs::msg::Transform current_goal_location; // x, y, z, yaw - geometry_msgs::msg::Transform last_location; // Last recorded position - rclcpp::Time last_position_change; // Time of last position change - double position_change_threshold = 0.1; // Minimum distance (meters) to consider as movement - double stall_timeout_seconds = 5.0; // Time without movement before clearing plan - - double cluster_cube_dim; - int voxel_cluster_count_thresh; - - double map_voxel_resolution; // map voxel grid size - - // Callbacks - // void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - - void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - - void mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg); - - void tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg); - - void vdbCallback(const std_msgs::msg::ByteMultiArray::SharedPtr msg); - - void ExplorationToggleCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - virtual void timerCallback(); - - virtual void generate_plan(); - - void publish_plan(); - - // Other functions - std::optional readParameters(); - -public: - // explicit ExplorationNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - ExplorationNode(); - ~ExplorationNode() = default; - - virtual void initialize(); - // TF buffer - std::shared_ptr tf_buffer; - std::shared_ptr tf_listener; - - std::shared_ptr> tf_filter_; - std::shared_ptr> lidar_filter_sub_; - - // ROS subscribers - rclcpp::Subscription::SharedPtr sub_lidar; - rclcpp::Subscription::SharedPtr sub_map; - rclcpp::Subscription::SharedPtr sub_vdb; - // rclcpp::Subscription::SharedPtr sub_robot_tf; - - // ROS publishers - rclcpp::Publisher::SharedPtr pub_global_plan; - rclcpp::Publisher::SharedPtr pub_goal_point; - rclcpp::Publisher::SharedPtr pub_trajectory_lines; - rclcpp::Publisher::SharedPtr pub_vdb; - rclcpp::Publisher::SharedPtr pub_frontier_vis; - rclcpp::Publisher::SharedPtr pub_clustered_frontier_vis; - rclcpp::Publisher::SharedPtr planning_debug_vis; - - // ROS services - rclcpp::Service::SharedPtr srv_exploration_toggle; - - // ROS timers - rclcpp::TimerBase::SharedPtr timer; -}; diff --git a/robot/ros_ws/src/global/planners/exploration/include/utils/collision_checker.h b/robot/ros_ws/src/global/planners/exploration/include/utils/collision_checker.h deleted file mode 100644 index aff0bb6f7..000000000 --- a/robot/ros_ws/src/global/planners/exploration/include/utils/collision_checker.h +++ /dev/null @@ -1,149 +0,0 @@ - -#ifndef COLLISION_CHECKER_H -#define COLLISION_CHECKER_H - -#include "utils/utils.hpp" -#include "utils/viewpoint.hpp" -#include "rclcpp/rclcpp.hpp" - -#include -#include - -// #include -// #include -// #include - -namespace collision_checker_ns -{ - // struct VirtualObstacle - // { - // ViewPoint point; - // float radius; - // rclcpp::Time expiry_time; - // }; - - class CollisionChecker - { - private: - // Constants - float kVoxSize_; - float kBboxLength_, kBboxBreadth_, kBboxHeight_; - float kL_occ_, kUnknown_fraction_thresh_, kOccupied_fraction_thresh_; - // bool kConsider_unknown_vox_occupied_; - // float kVirtual_obstacle_radius_, kVirtual_obstacle_expiry_time_; - // float kRobot_pos_clear_radius_; - // bool kEnable_gate_obstacle_; - // float kGate_cube_size_, kGate_wall_size_; - // std::string shared_frame_tf_topic_; - - // Stats counters - int count_hits_ = 0, count_misses_ = 0; - - openvdb::BoolGrid::Ptr collision_grid_ = NULL; - openvdb::FloatGrid::Ptr grid_map_ = NULL; - - // ros::Publisher debug_map_pub_, debug_virtual_obstacles_pub_, debug_virtual_gate_inner_pub_, debug_virtual_gate_outer_pub_; - // pcl::PointCloud::Ptr debug_map_; - // pcl::PointCloud::Ptr debug_virtual_obstacles_; - // pcl::PointCloud::Ptr debug_gate_obstacles_; - - // openvdb::math::Transform::Ptr darpa_vdb_tf_; - // tf::Transform darpa_tf_; - // bool darpa_tf_set_ = false; - - // openvdb::BoolGrid::Ptr gate_obstacle_inner_ = NULL; - // openvdb::BoolGrid::Ptr gate_obstacle_outer_ = NULL; - // bool inner_gate_obstacle_enabled_; - - // std::vector virtual_obstacles_; - - // ViewPoint robot_pos_; - // bool robot_pos_set_ = false; - - void initGrids(); - openvdb::Vec3d point2vdb(const ViewPoint &p_in); - void clearCache(); - // bool isNearOdometry(const ViewPoint &p); - // bool isInGateObstacle(const ViewPoint &p); - // void checkOdomInInnerGateObstacle(); - - static inline ViewPoint subtract(const ViewPoint &p1, const ViewPoint &p2) - { - ViewPoint p_return; - p_return.x = p1.x - p2.x; - p_return.y = p1.y - p2.y; - p_return.z = p1.z - p2.z; - return p_return; - } - - static inline ViewPoint addPoint(const ViewPoint &p1, const ViewPoint &p2) - { - ViewPoint p_return; - p_return.x = p1.x + p2.x; - p_return.y = p1.y + p2.y; - p_return.z = p1.z + p2.z; - return p_return; - } - - static inline ViewPoint divide(const ViewPoint &p, double factor) - { - ViewPoint p_return; - p_return.x = p.x / factor; - p_return.y = p.y / factor; - p_return.z = p.z / factor; - return p_return; - } - - static inline double norm(const ViewPoint &p) - { - return sqrt(p.x * p.x + p.y * p.y + p.z * p.z); - } - - static inline ViewPoint multiply(const ViewPoint &p, double factor) - { - ViewPoint p_return; - p_return.x = p.x * factor; - p_return.y = p.y * factor; - p_return.z = p.z * factor; - return p_return; - } - - public: - CollisionChecker(); - - void init(const float kVoxSize, - const float length, - const float breadth, - const float height, - const float l_occ, - const float unknown_fraction_thresh, - const float occupied_fraction_thresh); - - // void callbackDarpaTF(const nav_msgs::Odometry::ConstPtr &odom_msg); - // void markStartGate(); - - void updateGridPtr(const openvdb::FloatGrid::Ptr &grid); - void setOdometryForVirtualObstacles(const ViewPoint &robot_pos); - - bool estimateSurfaceNormalAtHit(const openvdb::Vec3d &hit_point_world, double inflation_factor, openvdb::Vec3d &normal_out); - bool collisionCheckRayStrict(const ViewPoint &p_start, const ViewPoint &p_end, openvdb::Vec3d& hit_point); - bool collisionCheckInFreeSpace(const ViewPoint &p); - bool collisionCheckInFreeSpaceVector(const ViewPoint &p_start, const ViewPoint &p_end); - bool collisionCheckInFreeSpaceVectorStepSize(const ViewPoint &p_start, const ViewPoint &p_end, const float step_size); - - // void publishCollisionCheckerViz(); - // void publishGateOuterObstacleViz(); - // void publishGateInnerObstacleViz(); - - // void addVirtualObstacles(const PointSet &obstacle_locations); - // void clearExpiredVirtualObstacles(); - // bool isInVirtualObstacle(const ViewPoint &p); - // void publishVirtualObstacleViz(); - - // bool gate_obstacle_created_ = false; - - }; // class CollisionChecker - -} // namespace collision_checker_ns - -#endif \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/include/utils/rrt_planner.h b/robot/ros_ws/src/global/planners/exploration/include/utils/rrt_planner.h deleted file mode 100644 index 4a6362363..000000000 --- a/robot/ros_ws/src/global/planners/exploration/include/utils/rrt_planner.h +++ /dev/null @@ -1,187 +0,0 @@ - -#ifndef RRT_PLANNER_H -#define RRT_PLANNER_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "utils/utils.hpp" -#include "utils/viewpoint.hpp" -#include "utils/collision_checker.h" - -// Define a Tree -struct Tree -{ - PointSet vertices; - std::map edges; // point, parent - ViewPoint goal; - - void clear() - { - vertices.clear(); - edges.clear(); - } - void insertPoint(const ViewPoint &new_point, const ViewPoint &parent) - { - vertices.push_back(new_point); - edges[new_point] = parent; - } -}; - -class RRT_Planner -{ -public: - RRT_Planner(); - bool RRT_Init( // const float robot_model_cube_dim, - // const openvdb::BoolGrid::Ptr& grid_map, - // const float l_occ, - // const float voxel_dim, - const float norm_limit, - const float max_explore_dist, - const int max_iter, - const int max_connection_iter, - const int smooth_horizon, - // const bool consider_unknown_vox_occupied, - const int rrt_region_nodes_count, - const float rrt_region_nodes_radius, - const int rrt_region_nodes_z_layers_count_up, - const float rrt_region_nodes_z_layers_step, - const float dense_step); - - bool build_RRT(const ViewPoint &start_point, const ViewPoint &end_point, - collision_checker_ns::CollisionChecker &collision_checker, const bool reset_trees = true); - PointSet getPath(); - PointSet getCoarsePath(); - // void updateGridPtr(const openvdb::BoolGrid::Ptr& grid); - bool visualizeTrees(); - // void setRobotModelCubeDim(const float robot_model_cube_dim){kRobot_model_cube_dim_ = robot_model_cube_dim;} - - // Tree Structure - Tree start_tree_; - Tree end_tree_; - - /* ---------------------------------------------------------------------------- */ -private: - // Debugging publishers - // ros::Publisher viz_tree_start_pub_; - // ros::Publisher viz_tree_end_pub_; - - // RRT constants - float dense_step_; - float kNorm_limit_, kNorm_limit_sq_; - float kMax_explore_dist_; - // float kRobot_model_cube_dim_; - // float kVoxel_dim_; - int kMax_iter_; - int kMax_connection_iter_; - int kSmooth_horizon_; - bool kConsider_unknown_vox_occupied_; - int kRRT_region_nodes_count_; - float kRRT_region_nodes_radius_; - int kRRT_region_nodes_z_layers_count_up_; - float kRRT_region_nodes_z_layers_step_; - - // Random number generator - std::uniform_real_distribution distribution_uniform_; - std::default_random_engine generator_; - - // RRT variables - ViewPoint start_point_, end_point_; - ViewPoint last_point_; - bool last_point_found_ = false; - - // Solution - PointSet path_; - PointSet coarse_path_; - - // Mapping variables - // openvdb::BoolGrid::Ptr grid_map_ = NULL; - // float kL_occ_; - - // Main functions - ViewPoint randomPoint(ViewPoint goal); - void interpolate(const ViewPoint &p1, const ViewPoint &p2, PointSet &path); - void pathSmooth(PointSet &path, collision_checker_ns::CollisionChecker &collision_checker); - ViewPoint nearestPoint(const ViewPoint &p_rand, const PointSet &vertices); - void extend(const ViewPoint &p_rand, const ViewPoint &p_near, ViewPoint &p_new); - bool validEdge(const ViewPoint &p_new, const ViewPoint &p_near, collision_checker_ns::CollisionChecker &collision_checker); - bool extractPathFromGraph(collision_checker_ns::CollisionChecker &collision_checker); - bool nearGoal(const ViewPoint &p_new, const ViewPoint &goal); - void addRegionVertices(Tree &tree); - // bool collisionCheckInFreeSpace(const ViewPoint& p); - void swapTreePointers(Tree **a, Tree **b); - void reversePath(PointSet &path); - - // ViewPoint auxillary functions - static inline ViewPoint subtract(const ViewPoint &p1, const ViewPoint &p2) - { - ViewPoint p_return; - p_return.x = p1.x - p2.x; - p_return.y = p1.y - p2.y; - p_return.z = p1.z - p2.z; - return p_return; - } - - static inline ViewPoint addPoint(const ViewPoint &p1, const ViewPoint &p2) - { - ViewPoint p_return; - p_return.x = p1.x + p2.x; - p_return.y = p1.y + p2.y; - p_return.z = p1.z + p2.z; - return p_return; - } - - static inline ViewPoint divide(const ViewPoint &p, double factor) - { - ViewPoint p_return; - p_return.x = p.x / factor; - p_return.y = p.y / factor; - p_return.z = p.z / factor; - return p_return; - } - - static inline double norm(const ViewPoint &p) - { - return sqrt(p.x * p.x + p.y * p.y + p.z * p.z); - } - - static inline double normSq(const ViewPoint &p) - { - // use instead of norm for efficiency and when you don't need the sqrt - return (p.x * p.x + p.y * p.y + p.z * p.z); - } - - static inline ViewPoint multiply(const ViewPoint &p, double factor) - { - ViewPoint p_return; - p_return.x = p.x * factor; - p_return.y = p.y * factor; - p_return.z = p.z * factor; - return p_return; - } - - static inline bool equal(const ViewPoint &p1, const ViewPoint &p2) - { - if (p1.x != p2.x) - { - return false; - } - if (p1.y != p2.y) - { - return false; - } - if (p1.z != p2.z) - { - return false; - } - return true; - } -}; - -#endif \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/include/utils/utils.hpp b/robot/ros_ws/src/global/planners/exploration/include/utils/utils.hpp deleted file mode 100644 index 66ff4406b..000000000 --- a/robot/ros_ws/src/global/planners/exploration/include/utils/utils.hpp +++ /dev/null @@ -1,138 +0,0 @@ -#ifndef _OLE_3D_UTIL_H_ -#define _OLE_3D_UTIL_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -using GridT = openvdb::Grid::Type>; - -// void generateVDBMarker(const GridT::Ptr& grid, -// const std::string& frame_id, -// visualization_msgs::msg::Marker& marker_msg); - -void generateVDBMarker(const openvdb::FloatGrid::Ptr &grid, - const std::string &frame_id, - visualization_msgs::msg::Marker &marker_msg); - -void generateFrontierMarker(const openvdb::BoolGrid::Ptr &grid, - const std::string &frame_id, - visualization_msgs::msg::Marker &marker_msg); - -void generateClusterMarker(const openvdb::FloatGrid::Ptr &grid, - const std::string &frame_id, - visualization_msgs::msg::Marker &marker_msg); - -bool surface_edge_frontier(openvdb::FloatGrid::Accessor accessor, openvdb::Coord ijk); - -void create_clustering_coordbbox(const openvdb::BoolGrid::Ptr grid, - openvdb::math::CoordBBox &coord_bbox_out, - const openvdb::Vec3d ¢er, - const float frontier_count_cube_dim); - -void point_clustering_vis(openvdb::BoolGrid::Ptr grid, - const float cluster_cube_dim, - pcl::PointCloud::Ptr cluster_cloud, - const int voxel_cluster_count_thresh, - std::vector> &clustered_points, - openvdb::FloatGrid::Ptr visualized_clusters); - -void mark_voxels_bool(const std::vector &point_list, - openvdb::BoolGrid::Ptr grid, - const bool voxel_value); - -class VDBUtil -{ -public: - static void updateOccMapFromNdArray(openvdb::FloatGrid::Ptr grid_logocc, - pcl::PointCloud::Ptr xyz, - const tf2::Vector3 &origin, - float l_free = -0.5f, // - a value if not hit - float l_occ = 1.0f, // + a value if hit - float l_min = -10.0f, // lower bound of free - float l_max = 10.0f, // upper bound of occ - int hit_thickness = 1, - double max_range = 50, - float visited_cleared_logocc_min_thresh = 8.0f); - - static void setVoxelSize(openvdb::GridBase &grid, double vs); - - static Eigen::Vector3d computeCentroid(const std::vector &point_list); - - static bool collCheckPointInPartialFreeSpace(const openvdb::FloatGrid::Ptr grid, - const Eigen::Vector3d &point_xyz, - const float length, - const float breadth, - const float height, - const float l_occ, - const float unknown_fraction_thresh, - const float occupied_fraction_thresh); - - static bool checkCollisionAlongRay(const openvdb::FloatGrid::Ptr& grid, - const openvdb::Vec3d &p1_xyz, - const openvdb::Vec3d &p2_xyz, - const float bbox_length, - const float bbox_breadth, - const float bbox_height, - const float l_occ, - openvdb::Vec3d &hit_point, - float dist_offset_from_cluster, - bool consider_unknown_occupied); - - static long int countVoxelsInsideBbox(const openvdb::FloatGrid::Ptr &grid, - const Eigen::Vector3d &bbox_center, - const float length, - const float breadth, - const float height); - - static void createCoordBBox(const openvdb::FloatGrid::Ptr grid, - openvdb::math::CoordBBox &coord_bbox_out, - const openvdb::Vec3d ¢er, - const float bbox_length, - const float bbox_breadth, - const float bbox_height); - - static bool checkIntersectWithCoordBBox(const openvdb::math::CoordBBox &search_bbox, - const openvdb::FloatGrid::Ptr grid, - float occupancy_threshold, - bool consider_unknown_occupied); - - static openvdb::Vec3d convertEigenVecToVec3D(const Eigen::Vector3d &point); - -}; // class VDBUtil -#endif \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/include/utils/viewpoint.hpp b/robot/ros_ws/src/global/planners/exploration/include/utils/viewpoint.hpp deleted file mode 100644 index 15041d4ab..000000000 --- a/robot/ros_ws/src/global/planners/exploration/include/utils/viewpoint.hpp +++ /dev/null @@ -1,318 +0,0 @@ -// Modified from cmusubt drone_traj_planner - -#ifndef CUSTOM_POINT_H -#define CUSTOM_POINT_H - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#define PI 3.14159265359 -#define TWO_PI 6.28318530718 - -// Define a Point -struct ViewPoint -{ - // Position - double x; - double y; - double z; - - bool orientation_set = false; - geometry_msgs::msg::Quaternion orientation; - double orientation_yaw; // equivalent to the yaw within orientation - - // Scoring - bool score_initialized = false; - double score; - bool cleared_by_other_robot = false; - - // Viewpoint score (from map processor) - bool viewpoint_score_initialized = false; - double viewpoint_score; -}; - -// Define a set of Points -typedef std::vector PointSet; - -// Define operators (for map to work) -static inline bool operator>(const ViewPoint &i, const ViewPoint &j) -{ - if (i.x > j.x) - { - return true; - } - else if (i.x == j.x) - { - if (i.y > j.y) - { - return true; - } - else if (i.y == j.y) - { - if (i.z > j.z) - { - return true; - } - else - { - return false; - } - } - else - { - return false; - } - } - else - { - return false; - } -} - -static inline bool operator<(const ViewPoint &i, const ViewPoint &j) -{ - if (i.x < j.x) - { - return true; - } - else if (i.x == j.x) - { - if (i.y < j.y) - { - return true; - } - else if (i.y == j.y) - { - if (i.z < j.z) - { - return true; - } - else - { - return false; - } - } - else - { - return false; - } - } - else - { - return false; - } -} - -static inline void printPoint(const ViewPoint &p) -{ - std::cout << "(" << p.x << ", " << p.y << ", " << p.z << ") {" << p.orientation.x << ", " << p.orientation.y << ", " << p.orientation.z << ", " << p.orientation.w << "}" << std::endl; -} - -static inline double distancePoint2Point(const ViewPoint &p1, const ViewPoint &p2) -{ - // Euclidean distance - double x_diff = p1.x - p2.x; - double y_diff = p1.y - p2.y; - double z_diff = p1.z - p2.z; - - return sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff); -} - -static inline double angleDiffPoint2Point(const ViewPoint &p1, const ViewPoint &p2) -{ - if (!p1.orientation_set || !p2.orientation_set) - { - return 0.0; - } - else - { - double angle_diff = p2.orientation_yaw - p1.orientation_yaw; - - // wrap around - if (angle_diff > PI) - { - angle_diff -= TWO_PI; - } - else if (angle_diff < -PI) - { - angle_diff += TWO_PI; - } - - return angle_diff; - } -} - -///////////////////// -// PCL -typedef pcl::PointXYZI PCLPointType; -typedef pcl::PointCloud PCLCloudType; - -static inline Eigen::Vector3d point2eigen(const ViewPoint& p) -{ - return Eigen::Vector3d(p.x, p.y, p.z); -} - -static inline openvdb::Vec3d point2vdb(const ViewPoint& p) -{ - return openvdb::Vec3d(p.x, p.y, p.z); -} - -static inline PCLPointType point2PCL(const ViewPoint &p) -{ - PCLPointType point; - point.x = p.x; - point.y = p.y; - point.z = p.z; - // point.intensity = p.score; - point.intensity = 0.0; - return point; -} - -static inline ViewPoint pcl2Point(const PCLPointType &pcl_point) -{ - ViewPoint point; - point.x = pcl_point.x; - point.y = pcl_point.y; - point.z = pcl_point.z; - return point; -} - -static inline PCLCloudType pointSet2PCL(const PointSet &ps) -{ - PCLCloudType pcl_cloud; - for (const auto &p : ps) - { - PCLPointType pcl_point = point2PCL(p); - pcl_cloud.push_back(pcl_point); - } - return pcl_cloud; -} - -///////////////////// -// Geometry msgs -static inline ViewPoint geometryMsgs2Point(const geometry_msgs::msg::Point &geom_point) -{ - ViewPoint point; - point.x = geom_point.x; - point.y = geom_point.y; - point.z = geom_point.z; - return point; -} - -static inline ViewPoint geometryMsgs2Point(const geometry_msgs::msg::Pose& geom_pose) -{ - ViewPoint point; - point.x = geom_pose.position.x; - point.y = geom_pose.position.y; - point.z = geom_pose.position.z; - - // tf2 (ROS 2-friendly) - double roll, pitch, yaw; - tf2::Quaternion quat_tf; - tf2::fromMsg(geom_pose.orientation, quat_tf); - tf2::Matrix3x3(quat_tf).getRPY(roll, pitch, yaw); - - point.orientation = geom_pose.orientation; - point.orientation_set = true; - point.orientation_yaw = yaw; - return point; -} - -static inline geometry_msgs::msg::Point point2GeometryMsgs(const ViewPoint &point) -{ - geometry_msgs::msg::Point geom_point; - geom_point.x = point.x; - geom_point.y = point.y; - geom_point.z = point.z; - return geom_point; -} - -static inline geometry_msgs::msg::Pose point2GeometryMsgsPose(const ViewPoint &point) -{ - geometry_msgs::msg::Pose geom_pose; - geom_pose.position.x = point.x; - geom_pose.position.y = point.y; - geom_pose.position.z = point.z; - if (point.orientation_set) - { - geom_pose.orientation = point.orientation; - } - else - { - geom_pose.orientation.x = 0.0; - geom_pose.orientation.y = 0.0; - geom_pose.orientation.z = 0.0; - geom_pose.orientation.w = 1.0; - } - return geom_pose; -} - -static inline geometry_msgs::msg::PoseArray pointSet2GeometryMsgsPoseArray(const PointSet &point_set) -{ - geometry_msgs::msg::PoseArray msg; - for (const auto &point : point_set) - { - msg.poses.push_back(point2GeometryMsgsPose(point)); - } - return msg; -} - -static inline nav_msgs::msg::Path pointSet2NavMsgsPath(const PointSet &point_set, const std_msgs::msg::Header &header) -{ - nav_msgs::msg::Path msg; - msg.header = header; - for (const auto &point : point_set) - { - geometry_msgs::msg::PoseStamped pose_stamped; - pose_stamped.header = header; - pose_stamped.pose = point2GeometryMsgsPose(point); - msg.poses.push_back(pose_stamped); - } - return msg; -} - -/////////////////// -// Viewpoints -static inline ViewPoint pclHSVtoPoint(const pcl::PointXYZHSV &pcl_point) -{ - ViewPoint point; - - // Position - point.x = pcl_point.x; - point.y = pcl_point.y; - point.z = pcl_point.z; - - // Score (from HSV 's' field) - point.viewpoint_score = pcl_point.s; - point.viewpoint_score_initialized = true; - - // Orientation from heading (HSV 'h' field) - tf2::Quaternion q; - q.setRPY(0.0, 0.0, pcl_point.h); // roll=0, pitch=0, yaw=heading - point.orientation = tf2::toMsg(q); - point.orientation_set = true; - - // Store yaw separately - point.orientation_yaw = pcl_point.h; - - return point; -} - -#endif \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/include/viewpoint_sampling.hpp b/robot/ros_ws/src/global/planners/exploration/include/viewpoint_sampling.hpp deleted file mode 100644 index 97a03b3a0..000000000 --- a/robot/ros_ws/src/global/planners/exploration/include/viewpoint_sampling.hpp +++ /dev/null @@ -1,121 +0,0 @@ -#ifndef _VIEWPOINT_SAMPLING_H_ -#define _VIEWPOINT_SAMPLING_H_ - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include "utils/utils.hpp" - -namespace viewpoint_sampling_ns -{ - class ViewpointSampling; -} // namespace viewpoint_sampling_ns - -class viewpoint_sampling_ns::ViewpointSampling -{ - -public: - using ListOfClustersT = std::vector>; - pcl::PointCloud::Ptr viewpoint_list_; - -private: - int cluster_idx_; - - // params - double viewing_distance_; - float bbox_length_, bbox_breadth_, bbox_height_; - float l_occ_, vox_size_; - uint16_t num_viewp_to_sample_; - double viewing_distance_inner_r_, viewing_distance_outer_r_, viewing_distance_z_offset_; - - // offset the endpoint for collision checking along the ray connecting the viewp to cluster centroid - // fraction of the robot model bbox so that the collision checking above is done for a small bbox - double coll_check_endpoint_offset_, bbox_fraction_viewp_centroid_coll_check_; - - // flags - uint8_t solver_type_; - - // viewpoint attributes - double current_cluster_score_, heading_; - double viewp_bbox_unknown_frac_thresh_, viewp_bbox_occ_frac_thresh_; - - // lists - std::shared_ptr cluster_list_; - std::shared_ptr> cluster_score_list_; - std::vector xy_points_; - pcl::PointCloud::Ptr centroid_list_; - std::vector line_y_points_, line_x_points_; - std::vector cluster_x_points_, cluster_y_points_; - - // current geometric entities - Eigen::Vector3d current_cluster_centroid_; - Eigen::Matrix opposing_viewpoints_; - Eigen::Vector3d direction_vector_; - Eigen::Vector2d fitted_line_; - Eigen::MatrixXd A_mat_; - Eigen::VectorXd b_vec_; - Eigen::Vector3d selected_viewpoint_; - openvdb::FloatGrid::Ptr grid_map_; - -public: - ViewpointSampling(ListOfClustersT &cluster_list, - double viewing_distance, - float bbox_length, - float bbox_breadth, - float bbox_height, - float l_occ, - float vox_size, - uint16_t num_viewp_sample_per_cluster, - double viewing_distance_inner_r, - double viewing_distance_outer_r, - double viewing_distance_z_offset, - double coll_check_endpoint_offset, - double bbox_fraction_viewp_centroid_coll_check, - double viewp_bbox_unknown_frac_thresh, - double viewp_bbox_occ_frac_thresh); - - // data handling - void updateGridPtr(const openvdb::FloatGrid::Ptr &grid); - void updateCentroidListPtr(const pcl::PointCloud::Ptr ¢roid_list); - void reset(ListOfClustersT &cluster_list); - - // utility methods - void sampleViewpoints(); - void projectToXYPlane(); - void computePointsPerpToLine(); - void selectOnePoint(); - float computeYawFromDirVector(); - bool fitLineToPoints(int solver_flag); - void useSVDDecomposition(); - void useQRDecomposition(); - void useNormalEquations(); - void createAMatrix(); - void createBMatrix(); - Eigen::Vector3d computeMean(); - pcl::PointXYZHSV constructViewpoint(); - void sampleViewpointsAroundCentroid(); - int sampleViewpointsAroundCentroidParamaterized(const double viewing_distance, - const double z_offset, - const double angle_spacing); - - // logging and plotting - std::string setupFilePath(int file_index, std::string file_name); - void writeToCSVfile(std::string file_name, Eigen::MatrixXd &matrix); - void writeVecToCSVfile(std::string file_name, - std::vector &points); - void logDataToFile(); - void plotPointsAndLine(); - void drawPointsAlongLine(float resolution, float length); - void toXVecYVec(); - -}; // class ViewpointSampling -#endif \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/launch/exploration_launch.xml b/robot/ros_ws/src/global/planners/exploration/launch/exploration_launch.xml deleted file mode 100644 index 24e9ee2ca..000000000 --- a/robot/ros_ws/src/global/planners/exploration/launch/exploration_launch.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/package.xml b/robot/ros_ws/src/global/planners/exploration/package.xml deleted file mode 100644 index b313f8ecc..000000000 --- a/robot/ros_ws/src/global/planners/exploration/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - exploration_planner - 0.0.0 - Frontier-based geometric exploration planner — kept as the intended alternative to random_walk for planner selection from the RViz Tasks Panel (future) - - Andrew Jong - - BSD-3-Clause-Clear - - ament_cmake - - rclcpp - rclcpp_action - std_msgs - std_srvs - geometry_msgs - nav_msgs - visualization_msgs - tf2 - tf2_ros - tf2_eigen - tf2_geometry_msgs - pcl_conversions - message_filters - - eigen - libpcl-all-dev - libopenvdb-dev - - - ament_cmake - - diff --git a/robot/ros_ws/src/global/planners/exploration/src/collision_checker.cpp b/robot/ros_ws/src/global/planners/exploration/src/collision_checker.cpp deleted file mode 100644 index 440b85690..000000000 --- a/robot/ros_ws/src/global/planners/exploration/src/collision_checker.cpp +++ /dev/null @@ -1,255 +0,0 @@ - -#include "utils/collision_checker.h" - -namespace collision_checker_ns -{ - - void CollisionChecker::init(const float kVoxSize, - const float length, - const float breadth, - const float height, - const float l_occ, - const float unknown_fraction_thresh, - const float occupied_fraction_thresh) - - { - kVoxSize_ = kVoxSize; - kBboxLength_ = length; - kBboxBreadth_ = breadth; - kBboxHeight_ = height; - kL_occ_ = l_occ; - kUnknown_fraction_thresh_ = unknown_fraction_thresh; - kOccupied_fraction_thresh_ = occupied_fraction_thresh; - - initGrids(); - } - - void CollisionChecker::initGrids() - { - // Grid used to cache collisions - collision_grid_ = openvdb::BoolGrid::create(false); - VDBUtil::setVoxelSize(*collision_grid_, kVoxSize_); - } - - inline openvdb::Vec3d CollisionChecker::point2vdb(const ViewPoint &p_in) - { - openvdb::Vec3d p_out(p_in.x, p_in.y, p_in.z); - return p_out; - } - - void CollisionChecker::updateGridPtr(const openvdb::FloatGrid::Ptr &grid) - { - grid_map_ = grid; - // clearExpiredVirtualObstacles(); - clearCache(); - } - - void CollisionChecker::clearCache() - { - collision_grid_->clear(); - // ROS_INFO_STREAM("hits: " << count_hits_ << " misses: " << count_misses_); - } - - bool CollisionChecker::collisionCheckRayStrict(const ViewPoint &p_start, const ViewPoint &p_end, openvdb::Vec3d &hit_point) - { - openvdb::Vec3d vec_start = point2vdb(p_start); - openvdb::Vec3d vec_end = point2vdb(p_end); - - return VDBUtil::checkCollisionAlongRay(grid_map_, vec_start, vec_end, - kBboxLength_, kBboxBreadth_, kBboxHeight_, - kL_occ_, hit_point, 0, false); - } - - bool CollisionChecker::collisionCheckInFreeSpace(const ViewPoint &p) - { - - // Convert to PCL point - Eigen::Vector3d query_pt = point2eigen(p); - - // Map does not exist - if (grid_map_ == NULL) - { - return false; - } - - // Setup cache accessor - openvdb::BoolGrid::Accessor collision_grid_acc = collision_grid_->getAccessor(); - const openvdb::math::Transform &collision_grid_tf(collision_grid_->transform()); - - openvdb::Vec3d cache_xyz = point2vdb(p); - openvdb::Coord cache_ijk = collision_grid_tf.worldToIndexCellCentered(cache_xyz); - - // Check cache - bool cache_hit = false; - bool cache_value; // only valid if cache_hit=true - if (!collision_grid_->empty()) - { - cache_hit = collision_grid_acc.probeValue(cache_ijk, cache_value); - } - - // Do collision checking using cache or explicit check - bool is_in_collision; - - if (cache_hit) - { - // Cache hit -- use stored value - count_hits_++; - is_in_collision = cache_value; - } - else - { - - // Cache miss -- do collision check - count_misses_++; - is_in_collision = !VDBUtil::collCheckPointInPartialFreeSpace(grid_map_, query_pt, - kBboxLength_, kBboxBreadth_, kBboxHeight_, - kL_occ_, kUnknown_fraction_thresh_, kOccupied_fraction_thresh_); - - // Add result to cache for next time - collision_grid_acc.setValueOn(cache_ijk, is_in_collision); - } - - // Ship it! - return is_in_collision; - } - - bool CollisionChecker::collisionCheckInFreeSpaceVector(const ViewPoint &p_start, const ViewPoint &p_end) - { - - // Map does not exist - if (grid_map_ == NULL) - { - return false; - } - - // Setup vector - ViewPoint check_point = p_start; - ViewPoint direction = subtract(p_end, p_start); - double diff = norm(direction); - direction = divide(direction, diff); - ViewPoint step = multiply(direction, double(kVoxSize_)); - - int num_steps = ceil(diff / kVoxSize_); - - // Step along the vector - for (int i = 0; i < num_steps; i++) - { - // Get next point on vector - check_point = addPoint(check_point, step); - - // Collision check - if (collisionCheckInFreeSpace(check_point)) - { - // Collision found - return true; - } - } - return false; - } - - // same as collisionCheckInFreeSpaceVector but with a custom step size - bool CollisionChecker::collisionCheckInFreeSpaceVectorStepSize(const ViewPoint &p_start, const ViewPoint &p_end, const float step_size) - { - - // Map does not exist - if (grid_map_ == NULL) - { - return false; - } - - // Setup vector - ViewPoint check_point = p_start; - ViewPoint direction = subtract(p_end, p_start); - double diff = norm(direction); - direction = divide(direction, diff); - ViewPoint step = multiply(direction, double(step_size)); - - int num_steps = ceil(diff / step_size); - - // Step along the vector - for (int i = 0; i < num_steps; i++) - { - - // Get next point on vector - check_point = addPoint(check_point, step); - - // Collision check - if (collisionCheckInFreeSpace(check_point)) - { - - // Collision found - return true; - } - } - return false; - } - - bool CollisionChecker::estimateSurfaceNormalAtHit(const openvdb::Vec3d &hit_point_world, - double inflation_factor, - openvdb::Vec3d &normal_out) - { - if (!grid_map_ || grid_map_->empty()) - { - return false; - } - - // Clamp inflation to non-negative. - const double infl = std::max(0.0, inflation_factor); - - // Build an inflated world-space AABB around the hit using your helper. - // We inflate the robot bbox uniformly by (1 + infl). - const float len = std::max(kVoxSize_, kBboxLength_ * static_cast(1.0 + infl)); - const float brd = std::max(kVoxSize_, kBboxBreadth_ * static_cast(1.0 + infl)); - const float hgt = std::max(kVoxSize_, kBboxHeight_ * static_cast(1.0 + infl)); - - openvdb::math::CoordBBox search_bbox; - VDBUtil::createCoordBBox(grid_map_, search_bbox, hit_point_world, len, brd, hgt); - - // Accessor & transform - openvdb::FloatGrid::Accessor acc = grid_map_->getAccessor(); - const openvdb::math::Transform &tf = grid_map_->transform(); - - // Accumulate centroid of OCCUPIED voxels (value > kL_occ_) inside the bbox. - openvdb::Vec3d sum_world(0.0, 0.0, 0.0); - std::size_t count = 0; - - for (auto it = search_bbox.beginXYZ(); it; ++it) - { - const openvdb::Coord ijk = *it; - - float v = 0.0f; - const bool active = acc.probeValue(ijk, v); - if (active && v > kL_occ_) - { - // Use voxel center in world space - sum_world += tf.indexToWorld(ijk); - ++count; - } - } - - if (count == 0) - { - // No occupied mass found in the (inflated) neighborhood — cannot infer a normal. - return false; - } - - const openvdb::Vec3d centroid_world = sum_world / static_cast(count); - - // Outward direction: from obstacle mass centroid -> hit point. - openvdb::Vec3d n = hit_point_world - centroid_world; - const double nlen = n.length(); - if (nlen <= 1e-9) - { - // Degenerate (e.g., centroid == hit); no stable direction. - return false; - } - - normal_out = n / nlen; // unit vector, world coordinates - return true; - } - - CollisionChecker::CollisionChecker() - { - } - -} // namespace collision_checker_ns \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/src/exploration_logic.cpp b/robot/ros_ws/src/global/planners/exploration/src/exploration_logic.cpp deleted file mode 100644 index 95633b0b9..000000000 --- a/robot/ros_ws/src/global/planners/exploration/src/exploration_logic.cpp +++ /dev/null @@ -1,762 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "../include/exploration_logic.hpp" - -ExplorationPlanner::ExplorationPlanner(init_params params) -{ - this->max_start_to_goal_dist_m_ = params.max_start_to_goal_dist_m; - this->checking_point_cnt = params.checking_point_cnt; - this->max_z_change_m_ = params.max_z_change_m; - this->voxel_size_m = params.voxel_size_m; - this->collision_padding_m = params.collision_padding_m; - this->path_end_threshold_m = params.path_end_threshold_m; - this->max_yaw_change_degrees = params.max_yaw_change_degrees; - - this->kLOcc_ = params.kLOcc_; - this->kVoxSize_ = params.kVoxSize_; - this->kViewingDistance_ = params.kViewingDistance_; - this->kBboxLength_ = params.kBboxLength_; - this->kBboxBreadth_ = params.kBboxBreadth_; - this->kBboxHeight_ = params.kBboxHeight_; - this->kNumViewpointsPerCluster_ = params.kNumViewpointsPerCluster_; - this->kViewingDistanceInnerR_ = params.kViewingDistanceInnerR_; - this->kViewingDistanceOuterR_ = params.kViewingDistanceOuterR_; - this->kViewingDistanceZOffset_ = params.kViewingDistanceZOffset_; - this->kCollCheckEndpointOffset_ = params.kCollCheckEndpointOffset_; - this->kRobotModelBBoxFraction_ = params.kRobotModelBBoxFraction_; - this->kViewpBBoxUnknownFracThresh_ = params.kViewpBBoxUnknownFracThresh_; - this->kViewpBBoxOccupiedFracThresh_ = params.kViewpBBoxOccupiedFracThresh_; - - this->too_close_distance_ = params.too_close_distance; - this->too_close_penalty_ = params.too_close_penalty; - this->odometry_match_distance_ = params.odometry_match_distance; - this->odometry_match_angle_ = params.odometry_match_angle; - this->odometry_match_penalty_ = params.odometry_match_penalty; - this->momentum_collision_check_distance_ = params.momentum_collision_check_distance; - this->momentum_collision_check_distance_min_ = params.momentum_collision_check_distance_min; - this->momentum_change_max_reward_ = params.momentum_change_max_reward; - this->momentum_change_collision_reward_ = params.momentum_change_collision_reward; - this->viewpoint_neighborhood_too_close_radius_ = params.viewpoint_neighborhood_too_close_radius; - this->viewpoint_neighborhood_sphere_radius_ = params.viewpoint_neighborhood_sphere_radius; - this->viewpoint_neighborhood_count_ = params.viewpoint_neighborhood_count; - this->momentum_minimum_distance_ = params.momentum_minimum_distance; - this->momentum_time_ = params.momentum_time; - this->momentum_collision_check_step_size_ = params.momentum_collision_check_step_size; - - this->planner_norm_limit_ = params.planner_norm_limit_; - this->max_explore_dist_ = params.max_explore_dist_; - this->planner_max_iter_ = params.planner_max_iter_; - this->max_connection_iter_ = params.max_connection_iter_; - this->traj_smooth_horizon_ = params.traj_smooth_horizon_; - this->rrt_region_nodes_count_ = params.rrt_region_nodes_count_; - this->rrt_region_nodes_radius_ = params.rrt_region_nodes_radius_; - this->rrt_region_nodes_z_layers_count_up_ = params.rrt_region_nodes_z_layers_count_up_; - this->rrt_region_nodes_z_layers_step_ = params.rrt_region_nodes_z_layers_step_; - - this->priority_level_thred_ = params.priority_level_thred_; - this->dense_step_ = params.dense_step_; - - viewp_sample_ = std::make_shared(clustered_points_, - kViewingDistance_, - kBboxLength_, - kBboxBreadth_, - kBboxHeight_, - kLOcc_, - kVoxSize_, - uint16_t(kNumViewpointsPerCluster_), - kViewingDistanceInnerR_, - kViewingDistanceOuterR_, - kViewingDistanceZOffset_, - kCollCheckEndpointOffset_, - kRobotModelBBoxFraction_, - kViewpBBoxUnknownFracThresh_, - kViewpBBoxOccupiedFracThresh_); - - collision_checker_.init(kVoxSize_, - kBboxLength_, - kBboxBreadth_, - kBboxHeight_, - kLOcc_, - kViewpBBoxUnknownFracThresh_, - kViewpBBoxOccupiedFracThresh_); - - rrt_planner_.RRT_Init(planner_norm_limit_, - max_explore_dist_, - planner_max_iter_, - max_connection_iter_, - traj_smooth_horizon_, - rrt_region_nodes_count_, - rrt_region_nodes_radius_, - rrt_region_nodes_z_layers_count_up_, - rrt_region_nodes_z_layers_step_, - dense_step_); -} - -std::optional ExplorationPlanner::plan_to_given_waypoint(const ViewPoint& start_point, const ViewPoint& goal_point) -{ - Path output_path; - bool is_traj = false; - - is_traj = rrt_planner_.build_RRT(start_point, goal_point, collision_checker_, true); - - // success - if (is_traj) - { - PointSet traj = rrt_planner_.getPath(); - output_path.reserve(traj.size()); - - // convert to our path format - for (const auto &vp : traj) - { - float yaw_val; - if (vp.orientation_set) - { - // Convert quaternion to yaw - tf2::Quaternion q(vp.orientation.x, vp.orientation.y, vp.orientation.z, vp.orientation.w); - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - yaw_val = static_cast(yaw); - } - else - { - yaw_val = static_cast(vp.orientation_yaw); // fallback if yaw is already set - } - - output_path.emplace_back(static_cast(vp.x), - static_cast(vp.y), - static_cast(vp.z), - yaw_val); - } - - RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "RRT Path generated"); - - return output_path; - } - - RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), - "No RRT path created"); - - return output_path; -} - -std::optional ExplorationPlanner::select_viewpoint_and_plan(const ViewPoint& start_point, float timeout_duration) -{ - RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "Start planning"); - robot_pos_ = start_point; - - this->sampled_viewpoints.clear(); - this->sampled_viewpoints.reserve(this->viewp_sample_->viewpoint_list_->size()); - - for (const auto &p : *this->viewp_sample_->viewpoint_list_) - { - this->sampled_viewpoints.push_back(pclHSVtoPoint(p)); - } - - RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "Computing Viewpoint Scores"); - - for (int viewpoint_index = 0; viewpoint_index < this->sampled_viewpoints.size(); viewpoint_index++) - { - double euclidean_distance = distancePoint2Point(start_point, this->sampled_viewpoints[viewpoint_index]); - double momentum_change_reward = 0; - double momentum_change_angle = angleBetweenHeadingAndMomentum(this->sampled_viewpoints[viewpoint_index]); - momentum_change_reward = (std::cos(momentum_change_angle) + 1.0) / 2.0; // 0 (behind) and 1 (in front) - double momentum_change_reward_before_collision_check = momentum_change_reward; - - if ((euclidean_distance >= momentum_collision_check_distance_min_ && euclidean_distance <= momentum_collision_check_distance_) && - (!collision_checker_.collisionCheckInFreeSpaceVector(start_point, this->sampled_viewpoints[viewpoint_index]))) - { - // Line of sight collision free, therefore add momentum reward - momentum_change_reward *= momentum_change_max_reward_; - } - else - { - // Line of sight blocked, therefore just add a small reward - // since path planning will likely require changing directions anyway - momentum_change_reward *= momentum_change_collision_reward_; - } - // score from viewpoint sampler - double map_processor_score = this->sampled_viewpoints[viewpoint_index].viewpoint_score; - - double score = -euclidean_distance + momentum_change_reward + map_processor_score; - - // Heavily penalize if viewpoint is too close to current location or lookahead point - if (euclidean_distance < too_close_distance_) - { - score -= too_close_penalty_ - 2.0 * euclidean_distance; - } - - // Similarly, reward if there's lots of viewpoints in cylinder between planning point and viewpoint - float cylinder_reward = 0.0; - if (euclidean_distance >= too_close_distance_) - { - cylinder_reward = getCylinderReward(viewpoint_index, start_point) * (0.25 + 0.75 * momentum_change_reward_before_collision_check); - } - score += cylinder_reward; - - // Heavily penalize if robot has already been to this viewpoint - bool already_been_to_viewpoint = doesPointMatchTrajectory(this->sampled_viewpoints[viewpoint_index]); - if (already_been_to_viewpoint) - { - // ROS_ERROR("Already been to this viewpoint!"); - score -= odometry_match_penalty_; - } - - if ((!this->sampled_viewpoints[viewpoint_index].score_initialized) || (score > this->sampled_viewpoints[viewpoint_index].score)) - { - this->sampled_viewpoints[viewpoint_index].score = score; - this->sampled_viewpoints[viewpoint_index].score_initialized = true; - - // Ignore the viewpoint if the map_processor score is not 200 (unobserved by other robots) or 150 (frontier of other robot) - // Also ignore if this robot has already been near it - if (map_processor_score < 100 || already_been_to_viewpoint == true) - // if (already_been_to_viewpoint) - { - this->sampled_viewpoints[viewpoint_index].cleared_by_other_robot = true; - } - } - } - - RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "RRT Start"); - // RRT to generate path - Path output_path; - bool is_traj = false; - - if (this->sampled_viewpoints.empty()) - { - RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), - "sampled_viewpoints empty"); - } - - for (int priority_level = 0; priority_level < priority_level_thred_ && priority_level < this->sampled_viewpoints.size(); priority_level++) - { - goalSelector(priority_level); - is_traj = rrt_planner_.build_RRT(start_point, explore_goal_, collision_checker_, true); - - // success - if (is_traj) - { - PointSet traj = rrt_planner_.getPath(); - output_path.reserve(traj.size()); - - // convert to our path format - for (const auto &vp : traj) - { - float yaw_val; - if (vp.orientation_set) - { - // Convert quaternion to yaw - tf2::Quaternion q(vp.orientation.x, vp.orientation.y, vp.orientation.z, vp.orientation.w); - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - yaw_val = static_cast(yaw); - } - else - { - yaw_val = static_cast(vp.orientation_yaw); // fallback if yaw is already set - } - - output_path.emplace_back(static_cast(vp.x), - static_cast(vp.y), - static_cast(vp.z), - yaw_val); - } - - RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "RRT Path generated"); - - return output_path; - } - } - - RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), - "No RRT path created"); - return output_path; -} - -void ExplorationPlanner::goalSelector(int priority_level) -{ - // INPUT : viewpoint_cloud, Graph, robot_pos_; - // OUTPUT: explore_goal_; - - if (priority_level == 0) - { - // assignAwardToFrontiers(); - std::sort(this->sampled_viewpoints.begin(), this->sampled_viewpoints.end(), compareFrontier); - } - - if (priority_level < this->sampled_viewpoints.size()) - { - explore_goal_ = this->sampled_viewpoints[priority_level]; - explore_goal_cleared_by_other_robot_ = this->sampled_viewpoints[priority_level].cleared_by_other_robot; - } - else - { - RCLCPP_WARN(rclcpp::get_logger("exploration_planner"), "priority level > num of sampled viewpoints"); - } -} - -bool ExplorationPlanner::compareFrontier(const ViewPoint &p1, const ViewPoint &p2) -{ - // Using this comparitor will sort from largest to smallest score - if (p1.score > p2.score) - { - return true; - } - return false; -} - -bool ExplorationPlanner::doesPointMatchTrajectory(const ViewPoint &point) -{ - // Does the point match any point in executed_trajectory_ - // within given tolerances - - double distance, angle; - - // Iterate through the executed trajectory, looking for a match - for (const ViewPoint &executed_point : executed_trajectory_) - { - - // Check if it matches - distance = distancePoint2Point(executed_point, point); - if (distance <= odometry_match_distance_) - { - - angle = angleDiffPoint2Point(executed_point, point); - if (angle >= -odometry_match_angle_ && angle <= odometry_match_angle_) - { - // Match found! - return true; - } - } - } - return false; -} - -bool ExplorationPlanner::extractCylinderViewpointDistances( - const int &viewpoint_index, const ViewPoint &planning_point, std::vector &viewpoint_distances) -{ - viewpoint_distances.clear(); - - double distance_pp_to_v = distancePoint2Point(planning_point, this->sampled_viewpoints[viewpoint_index]); - - // Only reward viewpoints that are between too_close_distance_ and viewpoint_neighborhood_too_close_radius_ - // distance from planning point - if ((distance_pp_to_v > viewpoint_neighborhood_too_close_radius_) || (distance_pp_to_v < too_close_distance_)) - { - return false; - } - - // Count how many neighbors - int count = 0; - - // Iterate through all viewpoints - // INCLUDING itself - for (int i = 0; i < this->sampled_viewpoints.size(); ++i) - { - double distance_pp_to_viewpoint = distancePoint2Point(planning_point, this->sampled_viewpoints[i]); - - double distance_odom_to_viewpoint = distancePoint2Point(robot_pos_, this->sampled_viewpoints[i]); - - // If this viewpoint is closer than the subject viewpoint but is not too_close to planning point - // Then this *might* be a neighbor - if ((distance_pp_to_viewpoint <= distance_pp_to_v + 0.01) && (distance_pp_to_viewpoint > too_close_distance_) && (distance_odom_to_viewpoint > too_close_distance_)) - { - - // Extract the cylinder distances - double parallel_distance, perpendicular_distance; - bool on_line_seg = lineSegDistances(this->sampled_viewpoints[i], planning_point, this->sampled_viewpoints[viewpoint_index], - parallel_distance, perpendicular_distance); - - // Is on line segment through center of cylinder - if (on_line_seg) - { - - // Check if in 45deg cone from planning_point - if (perpendicular_distance <= parallel_distance) - { - - // Check if within cylinder - if (perpendicular_distance <= viewpoint_neighborhood_sphere_radius_) - { - // Then viewpoint is considered a "neighbor" - ++count; - viewpoint_distances.push_back(parallel_distance); - } - } - } - } - } - - // Reward this viewpoint if lots of neighboring viewpoints found - return count >= viewpoint_neighborhood_count_; -} - -float ExplorationPlanner::getCylinderReward(const int &viewpoint_index, const ViewPoint &planning_point) -{ - // Find if there's a consecutive set of viewpoints in cylinder between planning_point and viewpoint - double distance_pp_to_v = distancePoint2Point(planning_point, this->sampled_viewpoints[viewpoint_index]); - - // Extract the distances from planning_point for all viewpoints in cylinder - std::vector viewpoint_distances; - bool has_viewpoint_neighborhood = extractCylinderViewpointDistances(viewpoint_index, planning_point, viewpoint_distances); - - if (!has_viewpoint_neighborhood) - { - // no reward - return 0.0; - } - - // Process these distances - - // Sort in DESCENDING order - std::sort(viewpoint_distances.begin(), viewpoint_distances.end(), std::greater()); - - // Search through the distance in descending order - // Looking for a "gap" - float gap_start = 0; - int count_neighbors = 0; - for (int i = 1; i < viewpoint_distances.size(); i++) - { - float previous = viewpoint_distances[i - 1]; - float current = viewpoint_distances[i]; - - float gap = previous - current; // previous is greater than current - - if (gap > 3.0) - { - // Gap found - if (i < viewpoint_neighborhood_count_) - { - // No reward - return 0.0; - } - - // Stop searching for gap - gap_start = previous; - count_neighbors = i; - break; - } - } - - return (viewpoint_distances[0] - gap_start) + 0.1 * (float)count_neighbors; -} - -bool lineSegDistances(const ViewPoint &point, const ViewPoint &line_segment_start, const ViewPoint &line_segment_end, - double ¶llel_distance, double &perpendicular_distance) -{ - // code adapted from http://geomalgorithms.com/a02-_lines.html - // Return false if point is outside of line segment - // Otherwise return true, along with the parallel and perpendicular distances - - // vector end to start - double v_x = line_segment_end.x - line_segment_start.x; - double v_y = line_segment_end.y - line_segment_start.y; - double v_z = line_segment_end.z - line_segment_start.z; - - // vector start to point - double w_x = point.x - line_segment_start.x; - double w_y = point.y - line_segment_start.y; - double w_z = point.z - line_segment_start.z; - - // if outside one boundary, return false - double c1 = v_x * w_x + v_y * w_y + v_z * w_z; // dot product - if (c1 <= 0) - return false; - - // if outside other boundary, return false - double c2 = v_x * v_x + v_y * v_y + v_z * v_z; // dot product - if (c2 <= c1) - return false; - - // otherwise project point and get distance (seems inefficient?) - double b = c1 / c2; - ViewPoint point_projected; - point_projected.x = line_segment_start.x + b * v_x; - point_projected.y = line_segment_start.y + b * v_y; - point_projected.z = line_segment_start.z + b * v_z; - - perpendicular_distance = distancePoint2Point(point, point_projected); - parallel_distance = distancePoint2Point(line_segment_start, point_projected); - return true; -} - -double ExplorationPlanner::angleBetweenHeadingAndMomentum(const ViewPoint &point) -{ - // Get the momentum vector - double momentum_direction_x, momentum_direction_y, momentum_direction_z; - bool momentum_computed = computeMomentumVector(momentum_direction_x, momentum_direction_y, momentum_direction_z); - - if (!momentum_computed) - { - // No momentum -- all viewpoints will be equally weighted in terms of momentum - return 0.0; - } - - // vector to point - double diff_x = point.x - robot_pos_.x; - double diff_y = point.y - robot_pos_.y; - double diff_z = point.z - robot_pos_.z; - double diff_norm = std::sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z); - - // avoid singularity - if (diff_norm < 0.01) - { - return 0.0; - } - - // normalize - diff_x /= diff_norm; - diff_y /= diff_norm; - diff_z /= diff_norm; - - // dot product - double dot_product = diff_x * momentum_direction_x + diff_y * momentum_direction_y + diff_z * momentum_direction_z; - - // cos(theta) = a.b / |a||b| - double relative_angle = std::acos(dot_product); - return relative_angle; -} - -bool ExplorationPlanner::computeMomentumVector(double &momentum_direction_x, double &momentum_direction_y, double &momentum_direction_z) -{ - // return false if no momentum computed - // momentum returned as a normalized (x,y) vector - // for now, assume z direction is 0, to encourage staying at same z level - - // Need at least two points to estimate momentum - if (this->momentum_executed_trajectory_.size() <= 1) - { - return false; - } - - // Get the start and end point of the vector - const ViewPoint ¤t_point = this->momentum_executed_trajectory_.back().point; - const ViewPoint &previous_point = this->momentum_executed_trajectory_.front().point; - - // Check not stationary first - double euclidean_distance = distancePoint2Point(current_point, previous_point); - if (euclidean_distance <= momentum_minimum_distance_) - { - return false; - } - - // Compute the momentum vector - momentum_direction_x = current_point.x - previous_point.x; - momentum_direction_y = current_point.y - previous_point.y; - momentum_direction_z = 0.0; - - // Normalize - double norm = std::sqrt(momentum_direction_x * momentum_direction_x + momentum_direction_y * momentum_direction_y + momentum_direction_z * momentum_direction_z); - - // avoid singularity (might happen since the above two distance measures are slightly different) - if (norm < 0.01) - { - return false; - } - - // normalize - momentum_direction_x /= norm; - momentum_direction_y /= norm; - momentum_direction_z /= norm; - - return true; -} - -// Junbin: Below are previous implementation to remove - -std::optional ExplorationPlanner::generate_straight_rand_path( - std::tuple start_point, float timeout_duration) -{ - // Locking mutex to prevent crashing when access the voxel map - std::optional path; - bool is_goal_point_valid = false; - // get start ti - const std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); - // RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "Starting Path Search..."); - - while (!is_goal_point_valid && std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time) - .count() < timeout_duration) - { - std::tuple goal_point = generate_goal_point(start_point); - // RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "Point Generated..."); - if (std::get<2>(goal_point) == -1) - { - RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), - "No valid goal point found in timeout duration"); - break; - } - - float x_diff = std::get<0>(goal_point) - std::get<0>(start_point); - float y_diff = std::get<1>(goal_point) - std::get<1>(start_point); - float z_diff = std::get<2>(goal_point) - std::get<2>(start_point); - float yaw = std::atan2(y_diff, x_diff); - - path = Path(); - std::tuple first_point( - std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point), yaw); - path.value().push_back(first_point); - - std::tuple current_point = std::make_tuple( - std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); - - // start moving in the direction of the goal point - while (get_point_distance(current_point, goal_point) > 0.001) - { - if (!check_if_collided(current_point)) - { - // add small delay to stop crashing - float new_x = std::get<0>(current_point) + x_diff / float(this->checking_point_cnt); - float new_y = std::get<1>(current_point) + y_diff / float(this->checking_point_cnt); - float new_z = std::get<2>(current_point) + z_diff / float(this->checking_point_cnt); - std::tuple new_point(new_x, new_y, new_z); - current_point = new_point; - std::tuple new_point_with_yaw( - std::get<0>(new_point), std::get<1>(new_point), std::get<2>(new_point), yaw); - path.value().push_back(new_point_with_yaw); - } - else - { - RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "Exploration: Collision detected"); - is_goal_point_valid = false; - break; - } - is_goal_point_valid = true; - } - } - if (!is_goal_point_valid) - { - RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "No Path found in time limit"); - return std::nullopt; - } - else - { - return path; - } -} - -bool ExplorationPlanner::check_if_collided_single_voxel( - const std::tuple &point, const std::tuple &voxel_center) -{ - // Check if the point is within the voxel - float x_max = std::get<0>(voxel_center) + std::get<0>(this->voxel_size_m) + this->collision_padding_m; - float x_min = std::get<0>(voxel_center) - std::get<0>(this->voxel_size_m) - this->collision_padding_m; - float y_max = std::get<1>(voxel_center) + std::get<1>(this->voxel_size_m) + this->collision_padding_m; - float y_min = std::get<1>(voxel_center) - std::get<1>(this->voxel_size_m) - this->collision_padding_m; - float z_max = std::get<2>(voxel_center) + std::get<2>(this->voxel_size_m) + this->collision_padding_m; - float z_min = std::get<2>(voxel_center) - std::get<2>(this->voxel_size_m) - this->collision_padding_m; - bool in_x = std::get<0>(point) < x_max && std::get<0>(point) > x_min; - bool in_y = std::get<1>(point) < y_max && std::get<1>(point) > y_min; - bool in_z = std::get<2>(point) < z_max && std::get<2>(point) > z_min; - if (in_x && in_y && in_z) - { - return true; - } - else - { - return false; - } -} - -bool ExplorationPlanner::check_if_collided(const std::tuple &point) -{ - // make sure the point is within the bounds -30 to 30 - if (std::get<0>(point) > 30 || std::get<0>(point) < -30 || std::get<1>(point) > 30 || - std::get<1>(point) < -30 || std::get<2>(point) > 30 || std::get<2>(point) < -30) - { - return true; - } - - std::lock_guard lock(this->mutex); - for (const std::tuple &voxel : this->voxel_points) - { - if (voxel == std::tuple(0, 0, 0)) - { - RCLCPP_INFO(rclcpp::get_logger("exploration_planner"), "Voxel is 0,0,0"); - } - if (check_if_collided_single_voxel(point, voxel)) - { - return true; - } - } - return false; -} - -std::tuple ExplorationPlanner::generate_goal_point( - std::tuple start_point) -{ - float time_out_duration = 1.0; - const clock_t start_time = clock(); - - while ((clock() - start_time) / CLOCKS_PER_SEC < time_out_duration) - { - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_real_distribution distribution_xy(-this->max_start_to_goal_dist_m_, - this->max_start_to_goal_dist_m_); - std::uniform_real_distribution distribution_z(-this->max_z_change_m_, this->max_z_change_m_); - float delta_x = distribution_xy(gen); - float delta_y = distribution_xy(gen); - float delta_z = distribution_z(gen); - float rand_x = std::get<0>(start_point) + delta_x; - float rand_y = std::get<1>(start_point) + delta_y; - float rand_z = std::get<2>(start_point) + delta_z; - rand_z = std::max(0.5f, rand_z); // ensure don't go below the ground - std::tuple rand_point(rand_x, rand_y, rand_z); - std::tuple start_point_wo_yaw( - std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); - float dist = get_point_distance(start_point_wo_yaw, rand_point); - float new_angle = std::atan2(std::get<1>(rand_point) - std::get<1>(start_point_wo_yaw), - std::get<0>(rand_point) - std::get<0>(start_point_wo_yaw)); - float angle_diff = std::abs(std::get<3>(start_point) - new_angle); - // if the z value of the point is low enough - if (angle_diff <= this->max_yaw_change_degrees * 3.14159265359 / 180) - { - // if the point is close enough to the start point - if (dist < this->max_start_to_goal_dist_m_) - { - // if the point doesnt collide with any of the voxels - if (!(check_if_collided(rand_point))) - { - return rand_point; - } - } - } - } - return std::tuple(0, 0, -1); -} - -double get_point_distance(const std::tuple &point1, - const std::tuple &point2) -{ - float x_diff = std::get<0>(point1) - std::get<0>(point2); - float y_diff = std::get<1>(point1) - std::get<1>(point2); - float z_diff = std::get<2>(point1) - std::get<2>(point2); - float dist = std::sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff); - if (std::isnan(dist) || std::isinf(dist)) - { - RCLCPP_ERROR(rclcpp::get_logger("exploration_planner"), "Distance is nan or inf"); - } - return dist; -} - -double deg2rad(double deg) { return deg * M_PI / 180.0; } - -double rad2deg(double rad) { return rad * 180.0 / M_PI; } diff --git a/robot/ros_ws/src/global/planners/exploration/src/exploration_node.cpp b/robot/ros_ws/src/global/planners/exploration/src/exploration_node.cpp deleted file mode 100644 index cdbc41a72..000000000 --- a/robot/ros_ws/src/global/planners/exploration/src/exploration_node.cpp +++ /dev/null @@ -1,916 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "../include/exploration_node.hpp" -#include "../include/exploration_logic.hpp" - -#include -#include -#include - -#include "utils/utils.hpp" - -std::optional ExplorationNode::readParameters() -{ - // Read in parameters based off the default yaml file - init_params params; - this->declare_parameter("robot_frame_id"); - if (!this->get_parameter("robot_frame_id", this->robot_frame_id_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: robot_frame_id"); - return std::optional{}; - } - this->declare_parameter("lidar_frame_id"); - if (!this->get_parameter("lidar_frame_id", this->lidar_frame_id_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: lidar_frame_id"); - return std::optional{}; - } - this->declare_parameter("map_frame_id"); - if (!this->get_parameter("map_frame_id", this->map_frame_id_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: map_frame_id"); - return std::optional{}; - } - this->declare_parameter("pub_global_plan_topic"); - if (!this->get_parameter("pub_global_plan_topic", this->pub_global_plan_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_global_plan_topic"); - return std::optional{}; - } - this->declare_parameter("pub_goal_point_viz_topic"); - if (!this->get_parameter("pub_goal_point_viz_topic", this->pub_goal_point_viz_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_goal_point_viz_topic"); - return std::optional{}; - } - this->declare_parameter("pub_trajectory_viz_topic"); - if (!this->get_parameter("pub_trajectory_viz_topic", this->pub_trajectory_viz_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_trajectory_viz_topic"); - return std::optional{}; - } - this->declare_parameter("pub_grid_viz_topic"); - if (!this->get_parameter("pub_grid_viz_topic", this->pub_grid_viz_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_grid_viz_topic"); - return std::optional{}; - } - this->declare_parameter("pub_frontier_viz_topic"); - if (!this->get_parameter("pub_frontier_viz_topic", this->pub_frontier_viz_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_frontier_viz_topic"); - return std::optional{}; - } - this->declare_parameter("pub_clustered_frontier_viz_topic"); - if (!this->get_parameter("pub_clustered_frontier_viz_topic", this->pub_clustered_frontier_viz_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_clustered_frontier_viz_topic"); - return std::optional{}; - } - this->declare_parameter("sub_lidar_topic"); - if (!this->get_parameter("sub_lidar_topic", this->sub_lidar_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_map_topic"); - return std::optional{}; - } - this->declare_parameter("sub_map_topic"); - if (!this->get_parameter("sub_map_topic", this->sub_map_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_map_topic"); - return std::optional{}; - } - this->declare_parameter("sub_vdb_topic"); - if (!this->get_parameter("sub_vdb_topic", this->sub_vdb_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_vdb_topic"); - return std::optional{}; - } - this->declare_parameter("sub_robot_tf_topic"); - if (!this->get_parameter("sub_robot_tf_topic", this->sub_robot_tf_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_robot_tf_topic"); - return std::optional{}; - } - this->declare_parameter("srv_exploration_toggle_topic"); - if (!this->get_parameter("srv_exploration_toggle_topic", this->srv_exploration_toggle_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: srv_exploration_toggle_topic"); - return std::optional{}; - } - this->declare_parameter("publish_visualizations"); - if (!this->get_parameter("publish_visualizations", this->publish_visualizations)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: publish_visualizations"); - return std::optional{}; - } - this->declare_parameter("num_paths_to_generate"); - if (!this->get_parameter("num_paths_to_generate", this->num_paths_to_generate_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: num_paths_to_generate"); - return std::optional{}; - } - this->declare_parameter("max_start_to_goal_dist_m"); - if (!this->get_parameter("max_start_to_goal_dist_m", params.max_start_to_goal_dist_m)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_start_to_goal_dist_m"); - return std::optional{}; - } - this->declare_parameter("checking_point_cnt"); - if (!this->get_parameter("checking_point_cnt", params.checking_point_cnt)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: checking_point_cnt"); - return std::optional{}; - } - this->declare_parameter("max_z_change_m"); - if (!this->get_parameter("max_z_change_m", params.max_z_change_m)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_change_m"); - return std::optional{}; - } - this->declare_parameter("collision_padding_m"); - if (!this->get_parameter("collision_padding_m", params.collision_padding_m)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: collision_padding_m"); - return std::optional{}; - } - this->declare_parameter("path_end_threshold_m"); - if (!this->get_parameter("path_end_threshold_m", params.path_end_threshold_m)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: path_end_threshold_m"); - return std::optional{}; - } - this->declare_parameter("max_yaw_change_degrees"); - if (!this->get_parameter("max_yaw_change_degrees", params.max_yaw_change_degrees)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_yaw_change_degrees"); - return std::optional{}; - } - this->declare_parameter("cluster_cube_dim"); - if (!this->get_parameter("cluster_cube_dim", this->cluster_cube_dim)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: cluster_cube_dim"); - return std::optional{}; - } - this->declare_parameter("voxel_cluster_count_thresh"); - if (!this->get_parameter("voxel_cluster_count_thresh", this->voxel_cluster_count_thresh)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: voxel_cluster_count_thresh"); - return std::optional{}; - } - this->declare_parameter("position_change_threshold"); - if (!this->get_parameter("position_change_threshold", this->position_change_threshold)) - { - - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: position_change_threshold"); - return std::optional{}; - } - this->declare_parameter("map_voxel_resolution"); - if (!this->get_parameter("map_voxel_resolution", this->map_voxel_resolution)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: map_voxel_resolution"); - return std::optional{}; - } - this->declare_parameter("stall_timeout_seconds"); - if (!this->get_parameter("stall_timeout_seconds", this->stall_timeout_seconds)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: stall_timeout_seconds"); - return std::optional{}; - } - - // viewpoint sampling - params.kLOcc_ = this->declare_parameter("l_occ", 1.0); - this->get_parameter("map_voxel_resolution", params.kVoxSize_); - params.kBboxLength_ = this->declare_parameter("bbox_length", 3.0); - params.kBboxBreadth_ = this->declare_parameter("bbox_breadth", 3.0); - params.kBboxHeight_ = this->declare_parameter("bbox_height", 3.0); - params.kViewingDistance_ = this->declare_parameter("viewing_distance", 3.0); - params.kNumViewpointsPerCluster_ = this->declare_parameter("num_viewpts_per_cluster", 6); - params.kViewingDistanceInnerR_ = this->declare_parameter("kViewingDistanceInnerR", 3.0); - params.kViewingDistanceOuterR_ = this->declare_parameter("kViewingDistanceOuterR", 6.0); - params.kViewingDistanceZOffset_ = this->declare_parameter("kViewingDistanceZOffset", 2.0); - params.kViewpBBoxUnknownFracThresh_ = this->declare_parameter("viewp_bbox_unknown_frac_thresh", 0.85); - params.kViewpBBoxOccupiedFracThresh_ = this->declare_parameter("viewp_bbox_known_occ_thresh", 0.0); - params.kCollCheckEndpointOffset_ = this->declare_parameter("coll_check_endpoint_offset", 0.4); - params.kRobotModelBBoxFraction_ = this->declare_parameter("robot_model_bbox_fraction", 0.1); - - // viewpoint selection - params.too_close_distance = this->declare_parameter("too_close_distance", 4.0); - params.too_close_penalty = this->declare_parameter("too_close_penalty", 5.0); - params.odometry_match_distance = this->declare_parameter("odometry_match_distance", 3.0); - params.odometry_match_angle = this->declare_parameter("odometry_match_angle", 3.14159); - params.odometry_match_penalty = this->declare_parameter("odometry_match_penalty", 1000.0); - params.momentum_collision_check_distance = this->declare_parameter("momentum_collision_check_distance", 15.0); - params.momentum_collision_check_distance_min = this->declare_parameter("momentum_collision_check_distance_min", 4.0); - params.momentum_change_max_reward = this->declare_parameter("momentum_change_max_reward", 6.0); - params.momentum_change_collision_reward = this->declare_parameter("momentum_change_collision_reward", 2.0); - params.viewpoint_neighborhood_too_close_radius = this->declare_parameter("viewpoint_neighborhood_too_close_radius", 10.0); - params.viewpoint_neighborhood_sphere_radius = this->declare_parameter("viewpoint_neighborhood_sphere_radius", 6.0); - params.viewpoint_neighborhood_count = this->declare_parameter("viewpoint_neighborhood_count", 5); - params.momentum_minimum_distance = this->declare_parameter("momentum_minimum_distance", 0.5); - params.momentum_time = this->declare_parameter("momentum_time", 2.0); - params.momentum_collision_check_step_size = this->declare_parameter("momentum_collision_check_step_size", 0.4); - - this->momentum_time_ = params.momentum_time; - // RRT - params.planner_norm_limit_ = this->declare_parameter("planner_norm_limit", 0.1); - params.max_explore_dist_ = this->declare_parameter("max_explore_dist", 100.0); - params.planner_max_iter_ = this->declare_parameter("planner_max_iter", 20000); - params.max_connection_iter_ = this->declare_parameter("max_connection_iter", 1000); - params.traj_smooth_horizon_ = this->declare_parameter("traj_smooth_horizon", 50); - params.rrt_region_nodes_count_ = this->declare_parameter("rrt_region_nodes_count", 16); - params.rrt_region_nodes_radius_ = this->declare_parameter("rrt_region_nodes_radius", 0.8); - params.rrt_region_nodes_z_layers_count_up_ = this->declare_parameter("rrt_region_nodes_z_layers_count_up", 1); - params.rrt_region_nodes_z_layers_step_ = this->declare_parameter("rrt_region_nodes_z_layers_step", 0.4); - - params.priority_level_thred_ = this->declare_parameter("priority_level_thred", 20); - params.dense_step_ = this->declare_parameter("dense_step", 0.1); - return params; -} - -ExplorationNode::ExplorationNode() : Node("exploration_node") -{ - // Initialize float 543 grid - openvdb::initialize(); - - // Initialize the exploration planner - std::optional params_opt = ExplorationNode::readParameters(); - - this->world_frame_id_ = this->map_frame_id_; - - if (params_opt.has_value()) - { - this->params = params_opt.value(); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize exploration planner"); - } - - this->params.voxel_size_m = std::tuple(map_voxel_resolution, map_voxel_resolution, map_voxel_resolution); - - // Initialize exploration planner - this->exploration_planner = std::make_unique(this->params); - RCLCPP_INFO(this->get_logger(), "Initialized exploration planner logic"); - - VDBUtil::setVoxelSize(*(this->exploration_planner->occ_grid), map_voxel_resolution); - VDBUtil::setVoxelSize(*(this->exploration_planner->prev_grid), map_voxel_resolution); - VDBUtil::setVoxelSize(*(this->exploration_planner->frontier_grid), map_voxel_resolution); - - RCLCPP_INFO(this->get_logger(), "Initialized exploration inner voxel grids"); -} - -void ExplorationNode::initialize() -{ - - // TF buffer and listener - tf_buffer = std::make_shared(this->get_clock()); - tf_listener = std::make_shared(*tf_buffer); - - this->sub_lidar = this->create_subscription( - sub_lidar_topic_, rclcpp::QoS(1).durability_volatile().best_effort(), std::bind(&ExplorationNode::lidarCallback, this, std::placeholders::_1)); - - // Trying message filter to avoid extrapolation when lookuptransform, not succedded yet - - // auto node_ptr = rclcpp::Node::shared_from_this(); - // RCLCPP_WARN(this->get_logger(), "Type: %s", typeid(*node_ptr).name()); - - // tf_buffer = std::make_shared(this->get_clock(), tf2::durationFromSec(10.0), node_ptr); - // tf_listener = std::make_shared(*tf_buffer); - - // lidar_filter_sub_ = std::make_shared>( - // this, sub_lidar_topic_, rmw_qos_profile_sensor_data); - - // tf_filter_ = std::make_shared>( - // *lidar_filter_sub_, *tf_buffer, map_frame_id_, 10, this->get_node_logging_interface(), this->get_node_clock_interface()); - - // tf_filter_->registerCallback(&ExplorationNode::lidarCallback, this); - - // Previous setup - - // this->sub_map = this->create_subscription( - // sub_map_topic_, 10, std::bind(&ExplorationNode::mapCallback, this, std::placeholders::_1)); - - // this->sub_vdb = this->create_subscription( - // sub_vdb_topic_, 10, std::bind(&ExplorationNode::vdbCallback, this, std::placeholders::_1)); - - this->pub_global_plan = this->create_publisher(pub_global_plan_topic_, 10); - this->pub_goal_point = - this->create_publisher(pub_goal_point_viz_topic_, 10); - this->pub_trajectory_lines = - this->create_publisher(pub_trajectory_viz_topic_, 10); - this->pub_vdb = - this->create_publisher(pub_grid_viz_topic_, 10); - this->pub_frontier_vis = - this->create_publisher(pub_frontier_viz_topic_, 10); - this->pub_clustered_frontier_vis = - this->create_publisher(pub_clustered_frontier_viz_topic_, 10); - this->planning_debug_vis = - this->create_publisher("/exploration_debug_vis", 10); - - // Set up the timer - this->timer = this->create_wall_timer(std::chrono::seconds(5), - std::bind(&ExplorationNode::timerCallback, this)); - // Set up the service - this->srv_exploration_toggle = this->create_service( - srv_exploration_toggle_topic_, std::bind(&ExplorationNode::ExplorationToggleCallback, this, - std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(this->get_logger(), "Exploration node initialized"); -} - -void ExplorationNode::ExplorationToggleCallback( - const std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (this->enable_exploration == false) - { - this->enable_exploration = true; - response->success = true; - response->message = "Exploration enabled"; - RCLCPP_INFO(this->get_logger(), "Exploration enabled"); - } - else - { - this->enable_exploration = false; - response->success = true; - response->message = "Exploration disabled"; - RCLCPP_INFO(this->get_logger(), "Exploration disabled"); - } -} - -void ExplorationNode::lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - geometry_msgs::msg::TransformStamped cloud_origin_tf; - - pcl::PointCloud::Ptr incoming_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *incoming_cloud); - - try - { - cloud_origin_tf = this->tf_buffer->lookupTransform( - map_frame_id_, lidar_frame_id_, msg->header.stamp, rclcpp::Duration(0, 100000000)); - // map_frame_id_, lidar_frame_id_, msg->header.stamp); - } - catch (tf2::TransformException &ex) - { - RCLCPP_ERROR(this->get_logger(), - "Could not transform %s to %s: %s", - map_frame_id_.c_str(), - lidar_frame_id_.c_str(), - ex.what()); - return; - } - - // pre-processing: remove nan - std::vector idx; - pcl::removeNaNFromPointCloud(*incoming_cloud, *incoming_cloud, idx); - - double min_range = 0.5; - - // pre-processing: remove too close points - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - tmp->reserve(incoming_cloud->size()); - const float r2min = min_range * min_range; - for (const auto &p : incoming_cloud->points) - { - const float r2 = p.x * p.x + p.y * p.y + p.z * p.z; - if (r2 >= r2min) - tmp->points.push_back(p); - } - tmp->width = static_cast(tmp->points.size()); - tmp->height = 1; - tmp->is_dense = true; - incoming_cloud.swap(tmp); - - pcl::transformPointCloud(*incoming_cloud, *incoming_cloud, tf2::transformToEigen(cloud_origin_tf).matrix()); - tf2::Vector3 origin_vec(cloud_origin_tf.transform.translation.x, - cloud_origin_tf.transform.translation.y, - cloud_origin_tf.transform.translation.z); - - VDBUtil::updateOccMapFromNdArray(this->exploration_planner->occ_grid, incoming_cloud, origin_vec); - this->exploration_planner->collision_checker_.updateGridPtr(this->exploration_planner->occ_grid); - - // visualization_msgs::msg::Marker vdb_marker_msg; - // generateVDBMarker(this->exploration_planner->occ_grid, map_frame_id_, vdb_marker_msg); - // vdb_marker_msg.header.stamp = this->now(); - // this->pub_vdb->publish(vdb_marker_msg); - - // RCLCPP_WARN(this->get_logger(), "Lidar Msg Received"); -} - -void ExplorationNode::mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg) -{ - // updating the local voxel points and generating a path only if the path is not executing - if (!this->received_first_map) - { - this->received_first_map = true; - this->world_frame_id_ = msg->header.frame_id; - RCLCPP_INFO(this->get_logger(), "Received first map"); - this->params.voxel_size_m = - std::tuple(msg->scale.x, msg->scale.y, msg->scale.z); - // this->exploration_planner = ExplorationPlanner(this->params); - this->exploration_planner = std::make_unique(this->params); - - RCLCPP_INFO(this->get_logger(), "Initialized exploration planner logic"); - } - this->exploration_planner->voxel_points.clear(); - for (int i = 0; i < msg->points.size(); i++) - { - this->exploration_planner->voxel_points.push_back( - std::tuple(msg->points[i].x, msg->points[i].y, msg->points[i].z)); - } -} - -void ExplorationNode::vdbCallback(const std_msgs::msg::ByteMultiArray::SharedPtr msg) -{ - std::string raw_data(reinterpret_cast(msg->data.data()), msg->data.size()); - std::istringstream iss(raw_data, std::ios::binary); - - openvdb::io::Stream in_stream(iss); - std::vector grids = *in_stream.getGrids(); - - if (grids.empty()) - { - RCLCPP_WARN(this->get_logger(), "Received VDB grid message, but no grids were found."); - return; - } - - using GridT = openvdb::Grid::Type>; - GridT::Ptr float_grid = openvdb::gridPtrCast(grids.front()); - - if (!float_grid) - { - RCLCPP_WARN(this->get_logger(), "First grid in received VDB message is not a FloatGrid."); - return; - } - - if (this->exploration_planner) - { - this->exploration_planner->vdb_grid = float_grid; - - if (!this->exploration_planner->first_grid_received) - { - this->exploration_planner->first_grid_received = true; - this->exploration_planner->frontier_grid->setTransform(float_grid->transform().copy()); - this->exploration_planner->prev_grid->setTransform(float_grid->transform().copy()); - RCLCPP_WARN(this->get_logger(), "Exploration Grids Initialized"); - } - // RCLCPP_WARN(this->get_logger(), "Successfully received and stored VDB FloatGrid (%zu active voxels).", - // this->exploration_planner->vdb_grid->activeVoxelCount()); - // RCLCPP_WARN(this->get_logger(), "received type is %s", grids.front()->type().c_str()); - // RCLCPP_WARN(this->get_logger(), "our type is %s", this->exploration_planner->vdb_grid->type().c_str()); - - // visualization_msgs::msg::Marker vdb_marker_msg; - // generateVDBMarker(this->exploration_planner->vdb_grid, "map", vdb_marker_msg); - // vdb_marker_msg.header.stamp = this->now(); - // this->pub_vdb->publish(vdb_marker_msg); - } - else - { - RCLCPP_WARN(this->get_logger(), "Exploration planner is not yet initialized."); - } -} - -void ExplorationNode::tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg) -{ - // get the current location - for (int i = 0; i < msg->transforms.size(); i++) - { - if (msg->transforms[i].child_frame_id.c_str() == this->robot_frame_id_) - { - this->current_location = msg->transforms[i].transform; - if (!this->received_first_robot_tf) - { - this->received_first_robot_tf = true; - RCLCPP_WARN(this->get_logger(), "Received first robot_tf"); - } - } - } -} - -void ExplorationNode::generate_plan() -{ - RCLCPP_INFO(this->get_logger(), "Starting to generate plan..."); - - // std::tuple start_loc; - // if (this->generated_paths.size() == 0) - // { - // geometry_msgs::msg::Quaternion orientation = this->current_location.rotation; - // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); - // q.normalize(); - // double roll, pitch, yaw; - // tf2::Matrix3x3 m(q); - // m.getRPY(roll, pitch, yaw); - // start_loc = std::make_tuple(this->current_location.translation.x, - // this->current_location.translation.y, - // this->current_location.translation.z, yaw); - // } - // else - // { - // geometry_msgs::msg::Quaternion orientation = - // this->generated_paths.back().poses.back().pose.orientation; - // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); - // q.normalize(); - // double roll, pitch, yaw; - // tf2::Matrix3x3 m(q); - // m.getRPY(roll, pitch, yaw); - // start_loc = std::make_tuple(this->generated_paths.back().poses.back().pose.position.x, - // this->generated_paths.back().poses.back().pose.position.y, - // this->generated_paths.back().poses.back().pose.position.z, yaw); - // } - - ViewPoint start_point; - start_point.x = this->current_location.translation.x; - start_point.y = this->current_location.translation.y; - start_point.z = this->current_location.translation.z; - - tf2::Quaternion q; - tf2::fromMsg(this->current_location.rotation, q); - q.normalize(); - - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - - start_point.orientation = this->current_location.rotation; - start_point.orientation_set = true; - start_point.orientation_yaw = yaw; - - // Convert start_loc to Point as starting point? - - // Replace the generate_straight_rand_path with the viewpoint score and RRT? - - float timeout_duration = 5.0; - std::optional gen_path_opt = - // this->exploration_planner->generate_straight_rand_path(start_loc, timeout_duration); - this->exploration_planner->select_viewpoint_and_plan(start_point, timeout_duration); - if (gen_path_opt.has_value() && gen_path_opt.value().size() > 0) - { - RCLCPP_INFO(this->get_logger(), "Generated path with %ld points", - gen_path_opt.value().size()); - - // set the current goal location - this->current_goal_location = geometry_msgs::msg::Transform(); - this->current_goal_location.translation.x = std::get<0>(gen_path_opt.value().back()); - this->current_goal_location.translation.y = std::get<1>(gen_path_opt.value().back()); - this->current_goal_location.translation.z = std::get<2>(gen_path_opt.value().back()); - float z_rot = std::get<3>(gen_path_opt.value().back()); - tf2::Quaternion q; - q.setRPY(0, 0, z_rot); // Roll = 0, Pitch = 0, Yaw = yaw - q.normalize(); - this->current_goal_location.rotation.x = q.x(); - this->current_goal_location.rotation.y = q.y(); - this->current_goal_location.rotation.z = q.z(); - this->current_goal_location.rotation.w = q.w(); - - // publish the path - nav_msgs::msg::Path generated_single_path; - generated_single_path = nav_msgs::msg::Path(); - generated_single_path.header.stamp = this->now(); - generated_single_path.header.frame_id = world_frame_id_; - for (auto point : gen_path_opt.value()) - { - geometry_msgs::msg::PoseStamped point_msg; - point_msg.pose.position.x = std::get<0>(point); - point_msg.pose.position.y = std::get<1>(point); - point_msg.pose.position.z = std::get<2>(point); - // convert yaw rotation to quaternion - tf2::Quaternion q; - q.setRPY(0, 0, std::get<3>(point)); // Roll = 0, Pitch = 0, Yaw = yaw - q.normalize(); - point_msg.pose.orientation.x = q.x(); - point_msg.pose.orientation.y = q.y(); - point_msg.pose.orientation.z = q.z(); - point_msg.pose.orientation.w = q.w(); - point_msg.header.stamp = this->now(); - generated_single_path.poses.push_back(point_msg); - } - geometry_msgs::msg::PoseStamped last_goal_loc = generated_single_path.poses.back(); - this->current_goal_location.translation.x = last_goal_loc.pose.position.x; - this->current_goal_location.translation.y = last_goal_loc.pose.position.y; - this->current_goal_location.translation.z = last_goal_loc.pose.position.z; - this->current_goal_location.rotation.z = last_goal_loc.pose.orientation.z; - this->generated_paths.push_back(generated_single_path); - - // debug vis of RRT - PointSet coarse_path = this->exploration_planner->rrt_planner_.getCoarsePath(); - visualization_msgs::msg::MarkerArray rrt_vis_array; - - if (coarse_path.empty()) - { - planning_debug_vis->publish(rrt_vis_array); - } - else - { - const std::string frame_id = "map"; - const rclcpp::Time stamp = this->now(); - - // ----- LINE_STRIP for the polyline ----- - visualization_msgs::msg::Marker line; - line.header.frame_id = frame_id; - line.header.stamp = stamp; - line.ns = "rrt_coarse_path"; - line.id = 0; - line.type = visualization_msgs::msg::Marker::LINE_STRIP; - line.action = visualization_msgs::msg::Marker::ADD; - line.pose.orientation.w = 1.0; // identity - line.scale.x = 0.03; // line width (m) - line.color.r = 0.1f; - line.color.g = 0.5f; - line.color.b = 1.0f; - line.color.a = 0.9f; - line.lifetime = rclcpp::Duration(0, 0); // forever - - line.points.reserve(coarse_path.size()); - for (const auto &v : coarse_path) - { - geometry_msgs::msg::Point p; - p.x = v.x; - p.y = v.y; - p.z = v.z; - line.points.push_back(p); - } - rrt_vis_array.markers.push_back(line); - - // ----- SPHERE_LIST for vertices ----- - visualization_msgs::msg::Marker verts; - verts.header.frame_id = frame_id; - verts.header.stamp = stamp; - verts.ns = "rrt_coarse_path"; - verts.id = 1; - verts.type = visualization_msgs::msg::Marker::SPHERE_LIST; - verts.action = visualization_msgs::msg::Marker::ADD; - verts.pose.orientation.w = 1.0; - verts.scale.x = 0.10; - verts.scale.y = 0.10; - verts.scale.z = 0.10; // sphere diameter (m) - verts.color.r = 1.0f; - verts.color.g = 0.2f; - verts.color.b = 0.2f; - verts.color.a = 0.9f; - verts.lifetime = rclcpp::Duration(0, 0); - - verts.points.reserve(coarse_path.size()); - for (const auto &v : coarse_path) - { - geometry_msgs::msg::Point p; - p.x = v.x; - p.y = v.y; - p.z = v.z; - verts.points.push_back(p); - } - rrt_vis_array.markers.push_back(verts); - - // publish - planning_debug_vis->publish(rrt_vis_array); - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to generate path, size was 0"); - } -} - -void ExplorationNode::publish_plan() -{ - nav_msgs::msg::Path full_path; - for (auto path : this->generated_paths) - { - for (auto point : path.poses) - { - full_path.poses.push_back(point); - } - } - full_path.header.stamp = this->now(); - full_path.header.frame_id = this->world_frame_id_; - this->pub_global_plan->publish(full_path); - RCLCPP_INFO(this->get_logger(), "Published full path"); -} - -void ExplorationNode::timerCallback() -{ - RCLCPP_INFO(this->get_logger(), "timer callback start"); - // get current TF to world - try - { - geometry_msgs::msg::TransformStamped transform_stamped = - this->tf_buffer->lookupTransform(this->world_frame_id_, this->robot_frame_id_, - rclcpp::Time(0)); - this->current_location = transform_stamped.transform; - if (!this->received_first_robot_tf) - { - this->received_first_robot_tf = true; - this->last_location = this->current_location; - this->last_position_change = this->now(); - RCLCPP_WARN(this->get_logger(), "Received first robot_tf"); - } - } - catch (tf2::TransformException &ex) - { - RCLCPP_ERROR(this->get_logger(), "Robot tf not received: %s", ex.what()); - } - - // Maintain the trajectory - ViewPoint vp; - vp.x = this->current_location.translation.x; - vp.y = this->current_location.translation.y; - vp.z = this->current_location.translation.z; - - vp.orientation.x = this->current_location.rotation.x; - vp.orientation.y = this->current_location.rotation.y; - vp.orientation.z = this->current_location.rotation.z; - vp.orientation.w = this->current_location.rotation.w; - - vp.orientation_set = true; - - double roll, pitch, yaw; - tf2::Quaternion tf2_q; - tf2::fromMsg(vp.orientation, tf2_q); // geometry_msgs -> tf2::Quaternion - tf2::Matrix3x3(tf2_q).getRPY(roll, pitch, yaw); - vp.orientation_yaw = yaw; - - this->exploration_planner->executed_trajectory_.push_back(vp); - this->exploration_planner->momentum_executed_trajectory_.push_back(TimedViewPoint{vp, this->now()}); - - // Prune old momentum samples (keep last momentum_time_ seconds) - const rclcpp::Time now = this->now(); - const rclcpp::Duration keep = rclcpp::Duration::from_seconds(this->momentum_time_); - - auto &traj = this->exploration_planner->momentum_executed_trajectory_; - while (!traj.empty() && (traj.front().time + keep) < now) - { - traj.erase(traj.begin()); - RCLCPP_INFO_ONCE(this->get_logger(), "Trim momentum traj"); - } - - // Update frontier map - - openvdb::Vec3d xyz; - openvdb::Coord ijk; - openvdb::FloatGrid::Accessor prev_acc = this->exploration_planner->prev_grid->getAccessor(); - openvdb::FloatGrid::Accessor lidar_acc = this->exploration_planner->occ_grid->getAccessor(); - openvdb::BoolGrid::Accessor frontier_acc = this->exploration_planner->frontier_grid->getAccessor(); - - for (auto iter = this->exploration_planner->frontier_grid->cbeginValueOn(); iter; ++iter) - { - ijk = iter.getCoord(); - // if the surrounding voxels are still frontier - bool still_front = surface_edge_frontier(lidar_acc, ijk); - if (!still_front) - { - frontier_acc.setValueOff(ijk, false); - } - } - // go through current free voxels - for (auto iter = this->exploration_planner->occ_grid->cbeginValueOn(); iter; ++iter) - { - ijk = iter.getCoord(); - openvdb::Vec3d xyz = this->exploration_planner->occ_grid->indexToWorld(ijk); - - // if (xyz.x() > explore_x_min_ && - // xyz.x() < explore_x_max_ && - // xyz.y() > explore_y_min_ && - // xyz.y() < explore_y_max_ && - // xyz.z() > explore_z_min_ && - // xyz.z() < explore_z_max_) - { - if (lidar_acc.getValue(ijk) < 0.0) - { // new free voxels - if (prev_acc.getValue(ijk) >= 0.0) - { - bool is_front = surface_edge_frontier(lidar_acc, ijk); - if (is_front) - { - frontier_acc.setValueOn(ijk, true); - } - } - } - } - } - - // update previous grid - this->exploration_planner->prev_grid = this->exploration_planner->occ_grid->deepCopy(); - - // clustering - openvdb::FloatGrid::Ptr visualized_cluster_grid_; - visualized_cluster_grid_ = openvdb::FloatGrid::create(0.0); - visualized_cluster_grid_->setTransform(this->exploration_planner->occ_grid->transform().copy()); - - std::vector> clustered_points_; - pcl::PointCloud::Ptr cluster_centroids_(new pcl::PointCloud); - - clustered_points_.clear(); - cluster_centroids_->clear(); - - point_clustering_vis(this->exploration_planner->frontier_grid, - this->cluster_cube_dim, - cluster_centroids_, - this->voxel_cluster_count_thresh, - clustered_points_, - visualized_cluster_grid_); - - // visualize - visualization_msgs::msg::Marker vdb_marker_msg; - generateVDBMarker(this->exploration_planner->occ_grid, map_frame_id_, vdb_marker_msg); - vdb_marker_msg.header.stamp = this->now(); - this->pub_vdb->publish(vdb_marker_msg); - - visualization_msgs::msg::Marker frontier_marker_msg; - generateFrontierMarker(this->exploration_planner->frontier_grid, map_frame_id_, frontier_marker_msg); - frontier_marker_msg.header.stamp = this->now(); - this->pub_frontier_vis->publish(frontier_marker_msg); - - visualization_msgs::msg::Marker clustered_marker_msg; - generateClusterMarker(visualized_cluster_grid_, map_frame_id_, clustered_marker_msg); - this->pub_clustered_frontier_vis->publish(clustered_marker_msg); - - // viewpoint sampling - this->exploration_planner->viewp_sample_->reset(clustered_points_); - this->exploration_planner->viewp_sample_->updateGridPtr(this->exploration_planner->occ_grid); - this->exploration_planner->viewp_sample_->updateCentroidListPtr(cluster_centroids_); - this->exploration_planner->viewp_sample_->sampleViewpoints(); - - RCLCPP_WARN(this->get_logger(), "Viewpoints Sampled %zu", this->exploration_planner->viewp_sample_->viewpoint_list_->size()); - - if (this->enable_exploration) - { - if (!this->is_path_executing) - { - if (this->exploration_planner->viewp_sample_->viewpoint_list_->size() > 0 && this->received_first_robot_tf) - { - - this->generate_plan(); - - this->publish_plan(); - this->is_path_executing = true; - this->last_position_change = this->now(); // Reset stall timer when starting new plan - } - else - { - RCLCPP_INFO(this->get_logger(), "viewpoint list size is %zu", this->exploration_planner->viewp_sample_->viewpoint_list_->size()); - RCLCPP_INFO(this->get_logger(), "tf received is %d", this->received_first_robot_tf); - } - } - else - { - // check if the robot has reached the goal point - std::tuple current_point = std::make_tuple( - this->current_location.translation.x, this->current_location.translation.y, - this->current_location.translation.z); - std::tuple goal_point = std::make_tuple( - this->current_goal_location.translation.x, this->current_goal_location.translation.y, - this->current_goal_location.translation.z); - if (get_point_distance(current_point, goal_point) < - this->exploration_planner->path_end_threshold_m) - { - this->is_path_executing = false; - this->generated_paths.clear(); - RCLCPP_INFO(this->get_logger(), "Reached goal point"); - } - else - { - // Check if position has changed significantly - std::tuple last_point = std::make_tuple(this->last_location.translation.x, - this->last_location.translation.y, - this->last_location.translation.z); - - if (get_point_distance(current_point, last_point) > this->position_change_threshold) - { - this->last_location = this->current_location; - this->last_position_change = this->now(); - } - else - { - // Check if we've been stationary for too long - rclcpp::Duration stall_duration = this->now() - this->last_position_change; - if (stall_duration.seconds() > this->stall_timeout_seconds) - { - RCLCPP_INFO(this->get_logger(), "Robot stationary for %f seconds, clearing plan", - stall_duration.seconds()); - this->is_path_executing = false; - this->generated_paths.clear(); - this->last_position_change = this->now(); // Reset timer to avoid spam - } - } - } - } - } - - RCLCPP_INFO(this->get_logger(), "timer callback end"); -} \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/src/exploration_node_run.cpp b/robot/ros_ws/src/global/planners/exploration/src/exploration_node_run.cpp deleted file mode 100644 index 99ac0c206..000000000 --- a/robot/ros_ws/src/global/planners/exploration/src/exploration_node_run.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "../include/exploration_node.hpp" - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->initialize(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/src/rrt_planner.cpp b/robot/ros_ws/src/global/planners/exploration/src/rrt_planner.cpp deleted file mode 100644 index 324742084..000000000 --- a/robot/ros_ws/src/global/planners/exploration/src/rrt_planner.cpp +++ /dev/null @@ -1,784 +0,0 @@ -/* - * Modified RRT Connection Function with Voxel Planner - * Copyright (C) 2020 Fan Yang - All rights reserved - * Original Author: fanyang2@andrew.cmu.edu - * Modified by: junbiny@andrew.cmu.edu - */ - -#include "utils/rrt_planner.h" - -// #define PI 3.14159265358979323846 // already in custom_point.h - -/* ---------------------------------------------------------------------------- */ -RRT_Planner::RRT_Planner() {} - -bool RRT_Planner::RRT_Init( // const float robot_model_cube_dim, - // const openvdb::BoolGrid::Ptr& grid_map, - // const float l_occ, - // const float voxel_dim, - const float norm_limit, - const float max_explore_dist, - const int max_iter, - const int max_connection_iter, - const int smooth_horizon, - // const bool consider_unknown_vox_occupied, - const int rrt_region_nodes_count, - const float rrt_region_nodes_radius, - const int rrt_region_nodes_z_layers_count_up, - const float rrt_region_nodes_z_layers_step, - const float dense_step) -{ - // Initialization - kSmooth_horizon_ = smooth_horizon; - // kRobot_model_cube_dim_ = robot_model_cube_dim; - kNorm_limit_ = norm_limit; - dense_step_ = dense_step; - kNorm_limit_sq_ = kNorm_limit_ * kNorm_limit_; - // kVoxel_dim_ = voxel_dim; - kMax_explore_dist_ = max_explore_dist; - kMax_iter_ = max_iter; - kMax_connection_iter_ = max_connection_iter; - // kConsider_unknown_vox_occupied_ = consider_unknown_vox_occupied; - - // Clear graph - start_tree_.clear(); - end_tree_.clear(); - - last_point_found_ = false; - - // grid_map_ = openvdb::BoolGrid::Ptr(grid_map); - // kL_occ_ = l_occ; - - kRRT_region_nodes_count_ = rrt_region_nodes_count; - kRRT_region_nodes_radius_ = rrt_region_nodes_radius; - kRRT_region_nodes_z_layers_count_up_ = rrt_region_nodes_z_layers_count_up; - kRRT_region_nodes_z_layers_step_ = rrt_region_nodes_z_layers_step; - - // Set seed - unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); - generator_ = std::default_random_engine(seed); - distribution_uniform_ = std::uniform_real_distribution(0.0, 1.0); - - // Debugging publishers - // ros::NodeHandle nh; - // viz_tree_start_pub_ = nh.advertise("viz_rrt_tree_start", 1); - // viz_tree_end_pub_ = nh.advertise("viz_rrt_tree_end", 1); - - return true; -} - -void RRT_Planner::addRegionVertices(Tree &tree) -{ - // Assumes edges has been initialized with one node - // Add new nodes, without collision checking, around this point - // These node form a cylinder - - if (kRRT_region_nodes_count_ > 0) - { - - // Get the existing point - const ViewPoint &point_center = tree.vertices[0]; - - float two_PI_over_count = 2.0 * PI / (float)kRRT_region_nodes_count_; - - // Add new points on a circle around the center - for (int i = 0; i < kRRT_region_nodes_count_; i++) - { - - float theta = (float)i * two_PI_over_count; - - float x = point_center.x + kRRT_region_nodes_radius_ * cos(theta); - float y = point_center.y + kRRT_region_nodes_radius_ * sin(theta); - - for (int j = -kRRT_region_nodes_z_layers_count_up_; j <= kRRT_region_nodes_z_layers_count_up_; j++) - { - // Create a new point for each z layer - ViewPoint point_new; - point_new.x = x; - point_new.y = y; - point_new.z = point_center.z + j * kRRT_region_nodes_z_layers_step_; - - // Ship it - tree.insertPoint(point_new, point_center); - } - } - } -} - -bool RRT_Planner::build_RRT(const ViewPoint &start_point, - const ViewPoint &end_point, collision_checker_ns::CollisionChecker &collision_checker, const bool reset_trees) -{ - if (collision_checker.collisionCheckInFreeSpace(start_point)) - { - std::cout << "Starting Point Not Free Enough \n"; - } - - if (collision_checker.collisionCheckInFreeSpace(end_point)) - { - std::cout << "End Point Not Free Enough \n"; - } - - // Initialize the trees - if (reset_trees) - { - start_tree_.clear(); - end_tree_.clear(); - } - - // Initialize flags - last_point_found_ = false; - bool is_path = false; - bool is_trace_path = true; - - // Save start and end points in tree - start_point_ = start_point; - end_point_ = end_point; - start_tree_.goal = end_point_; - end_tree_.goal = start_point_; - - if (reset_trees) - { - start_tree_.vertices.push_back(start_point_); - end_tree_.vertices.push_back(end_point_); - - // Add region vertices - addRegionVertices(start_tree_); - addRegionVertices(end_tree_); - } - - // Intermediate points - ViewPoint p_random; - ViewPoint p_nearest, p_new, p_nearest_goal, p_new_goal; - - // Create pointers to the two trees so they can be efficiently swapped - Tree *treeA = &start_tree_; - Tree *treeB = &end_tree_; - - // Iterate until goal found or max_iterations - for (int iteration = 0; iteration < kMax_iter_; ++iteration) - { - - // Pick a new random point - p_random = randomPoint(treeA->goal); - - // Get the closest point in graph to random point - p_nearest = nearestPoint(p_random, treeA->vertices); - - // Extend towards random point - extend(p_random, p_nearest, p_new); - - if (validEdge(p_new, p_nearest, collision_checker)) - { - - // Save the new vertex - treeA->insertPoint(p_new, p_nearest); - - // Extend from goal, too - p_nearest_goal = nearestPoint(p_new, treeB->vertices); - extend(p_new, p_nearest_goal, p_new_goal); // direction; start; new - ViewPoint p_new2; - if (validEdge(p_new_goal, p_nearest_goal, collision_checker)) - { - - // Save the new vertex - treeB->insertPoint(p_new_goal, p_nearest_goal); - - // Try to connect to the goal - int connection_iters = 0; - while (!nearGoal(p_new_goal, p_new) && connection_iters <= kMax_connection_iter_) - { - - // Extend towards goal - extend(p_new, p_new_goal, p_new2); - - if (validEdge(p_new2, p_new_goal, collision_checker)) - { - - // Save the new vertex - treeB->insertPoint(p_new2, p_new_goal); - p_new_goal = p_new2; - } - else - { - break; - } - - // Increment loop counter - connection_iters++; - } - if (connection_iters > kMax_connection_iter_) - { - printPoint(p_new); - printPoint(p_new_goal); - } - } - - // Check termination condition, add edge between trees (sort of) - if (nearGoal(p_new_goal, p_new)) - { - // treeB->edges[p_new] = p_new_goal; - treeB->insertPoint(p_new, p_new_goal); - last_point_ = p_new; - last_point_found_ = true; - is_path = true; - break; - } - } - - // Use the goal tree if it was better - if (treeA->vertices.size() > treeB->vertices.size()) - { - swapTreePointers(&treeA, &treeB); - } - } - - // Construct the path from the graph - if (is_path) - { - path_.clear(); - coarse_path_.clear(); - is_trace_path = extractPathFromGraph(collision_checker); - if (!is_trace_path) - { - path_.clear(); - } - } - - return (is_path && is_trace_path); -} - -// bool RRT_Planner::visualizeTrees() -// { -// // Convert both trees to point cloud msgs -// // So they can be viewed in rviz -// // (just the vertices, not the edges) - -// // First tree -// PCLCloudType pcl_cloud_start = pointSet2PCL(start_tree_.vertices); -// sensor_msgs::PointCloud2Ptr pcl_cloud_start_ros(new sensor_msgs::PointCloud2); -// pcl::toROSMsg(pcl_cloud_start, *pcl_cloud_start_ros); -// pcl_cloud_start_ros->header.frame_id = "map"; -// pcl_cloud_start_ros->header.stamp = ros::Time::now(); -// viz_tree_start_pub_.publish(pcl_cloud_start_ros); - -// // Second tree -// PCLCloudType pcl_cloud_end = pointSet2PCL(end_tree_.vertices); -// sensor_msgs::PointCloud2Ptr pcl_cloud_end_ros(new sensor_msgs::PointCloud2); -// pcl::toROSMsg(pcl_cloud_end, *pcl_cloud_end_ros); -// pcl_cloud_end_ros->header.frame_id = "map"; -// pcl_cloud_end_ros->header.stamp = ros::Time::now(); -// viz_tree_end_pub_.publish(pcl_cloud_end_ros); -// } - -/*bool RRT_Planner::collisionCheckInFreeSpace(const ViewPoint& p) { - bool is_in_collision; - // TODO - this conversion is unnecessary -> see second comment below - pcl::PointXYZI query_pt = point2PCL(p); - // LEGACY DEBT - TODO convert method to new point type - if(grid_map_ == NULL) { - // std::cout << "[RRT_Planner::collisionCheckInFreeSpace] Grid ptr within class is NULL - not initialized." << "\n"; - return false; - } - - if(!vdbmap::VDBUtil::checkPointInFreeSpace(grid_map_, query_pt, - kRobot_model_cube_dim_, kL_occ_, - kConsider_unknown_vox_occupied_)) { - is_in_collision = true; - return is_in_collision; - } else { - is_in_collision = false; - return is_in_collision; - } -}*/ - -void RRT_Planner::swapTreePointers(Tree **a, Tree **b) -{ - - // Swap the tree pointers - Tree *temp_tree = *a; - *a = *b; - *b = temp_tree; -} - -ViewPoint RRT_Planner::randomPoint(ViewPoint goal) -{ - - // With some probability, select goal instead of random point - double r = distribution_uniform_(generator_); - if (r < 0.90) - { - - // New random point - ViewPoint p; - float range, range_z; - - float dist = norm(subtract(end_point_, start_point_)); - - if (r < 0.50) - { - // Sample more at closer range - range = (dist + 3) * 1.5; - range_z = (end_point_.z - start_point_.z + 3) * 1.5; - } - else - { - // Wider sample range - range = (dist + 5) * 2.0; - range_z = (end_point_.z - start_point_.z + 3) * 2.0; - } - - p.x = (distribution_uniform_(generator_) - 0.5) * range + (start_point_.x + end_point_.x) / 2.0; - p.y = (distribution_uniform_(generator_) - 0.5) * range + (start_point_.y + end_point_.y) / 2.0; - p.z = (distribution_uniform_(generator_) - 0.5) * range_z + (start_point_.z + end_point_.z) / 2.0; - - return p; - } - else - { - - // Select the goal instead - return goal; - } -} - -void RRT_Planner::reversePath(PointSet &path) -{ - // Reverse the order of the trajectory, save in place - - std::reverse(path.begin(), path.end()); -} - -void RRT_Planner::extend(const ViewPoint &p_rand, const ViewPoint &p_near, ViewPoint &p_new) -{ - // Extend from p_near towards p_rand - // Return the extended point p_new - - // Get unit vector in direction - ViewPoint direction = subtract(p_rand, p_near); - double diff = norm(direction); - direction = divide(direction, diff); - - // Extend along unit vector, without going past p_rand - ViewPoint dist_step = multiply(direction, fmin(kNorm_limit_, diff)); - - // Get the new point - p_new = addPoint(p_near, dist_step); -} - -// bool RRT_Planner::validEdge(const ViewPoint &p_new, const ViewPoint &p_near, collision_checker_ns::CollisionChecker &collision_checker) -// { -// // Collision checking at discrete points between the two points? - -// ViewPoint check_point = p_near; -// ViewPoint direction = subtract(p_new, p_near); -// double diff = norm(direction); -// direction = divide(direction, diff); -// ViewPoint step = multiply(direction, double(kNorm_limit_)); // kVoxel_dim_ - -// int num_steps = ceil(diff / kNorm_limit_); // kVoxel_dim_ - -// // while(norm(subtract(check_point,p_near)) < diff) { -// for (int i = 0; i < num_steps; i++) -// { -// check_point = addPoint(check_point, step); -// if (collision_checker.collisionCheckInFreeSpace(check_point)) -// { -// return false; -// } -// } -// return true; -// } - -bool RRT_Planner::validEdge(const ViewPoint &p_new, const ViewPoint &p_near, collision_checker_ns::CollisionChecker &collision_checker) -{ - openvdb::Vec3d hit_point; - if (collision_checker.collisionCheckRayStrict(p_new, p_near, hit_point)) - { - return false; - } - return true; -} - -ViewPoint RRT_Planner::nearestPoint(const ViewPoint &p_rand, const PointSet &vertices) -{ - // Get the nearest vertex - // TODO KD-tree? - - bool first = true; - double min_norm_dist_sq = 0; - ViewPoint p_near; - - // Iterate through the vertices - for (const auto &vertex : vertices) - { - - // Compute the distance - double norm_dist_sq = normSq(subtract(vertex, p_rand)); - if (first || (norm_dist_sq < min_norm_dist_sq)) - { - min_norm_dist_sq = norm_dist_sq; - p_near = vertex; - first = false; - } - } - return p_near; -} - -inline bool RRT_Planner::nearGoal(const ViewPoint &p_new, const ViewPoint &goal) -{ - // Check if p_new is sufficiently close to the goal - - ViewPoint diff = subtract(goal, p_new); - double norm_dist_sq = normSq(diff); - if (norm_dist_sq < kNorm_limit_sq_) - { - return true; - } - return false; -} - -bool RRT_Planner::extractPathFromGraph(collision_checker_ns::CollisionChecker &collision_checker) -{ - // Extract path from graph - - bool is_trace_success = true; - - // Was the goal found? - if (!last_point_found_) - { - - // If not, stay stationary - path_.push_back(start_point_); - } - else - { - - // Found goal -- build path along graph - - // Initialize stuff - ViewPoint current; - PointSet path; - current = last_point_; - path.push_back(current); - std::size_t n_start_edges = start_tree_.edges.size(); - - // Construct a path - if (start_tree_.edges.count(current)) - { - - bool solution_found = false; - - // Upper bound to avoid infinite loop - for (int i = 0; i < 2 * n_start_edges; ++i) - { - - // Find parent - auto next_edge = start_tree_.edges.find(current); - if (next_edge != start_tree_.edges.end()) - { - - // Follow parent - current = next_edge->second; - path.push_back(current); - } - else - { - - // Reached root - solution_found = true; - break; - } - - // This condition appears to check for infinite loops? - // if (start_tree_.edges.find(parent) != start_tree_.edges.end() && equal(start_tree_.edges.find(parent)->second,current)) { - // is_trace_success = false; - // return false; - // }else{ - // Follow parent - // current = parent; - // } - // path.push_back(current); - } - if (!solution_found) - { - // Something has gone wrong - is_trace_success = false; - return false; - } - } - else - { - // Something has gone wrong - is_trace_success = false; - return false; - } - - // Do the same for end_edges - - // Initialize stuff - ViewPoint current1; - PointSet path1; - current1 = last_point_; - path1.push_back(current1); - std::size_t n_end_edges = end_tree_.edges.size(); - - // Construct a path - if (end_tree_.edges.count(current1)) - { - - bool solution_found = false; - - // Upper bound to avoid infinite loop - for (int i = 0; i < 2 * n_end_edges; ++i) - { - - // Find parent - auto next_edge = end_tree_.edges.find(current1); - if (next_edge != end_tree_.edges.end()) - { - - // Follow parent - current1 = next_edge->second; - path1.push_back(current1); - } - else - { - - // Reached root - solution_found = true; - break; - } - } - if (!solution_found) - { - // Something has gone wrong - is_trace_success = false; - return false; - } - } - else - { - // Something has gone wrong - is_trace_success = false; - return false; - } - - // Stitch the two paths together - if (equal(current, start_point_)) - { - reversePath(path); - path_.insert(path_.end(), path.begin(), path.end()); - path_.insert(path_.end(), path1.begin(), path1.end()); - } - else - { - // This case should no longer happen (since trees don't get directly swapped any more) - reversePath(path1); - path_.insert(path_.end(), path1.begin(), path1.end()); - path_.insert(path_.end(), path.begin(), path.end()); - } - } - - // Error checking - int path_size = path_.size(); - if (!equal(path_[0], start_point_) || !equal(path_[path_size - 1], end_point_)) - { - // Something has gone wrong - is_trace_success = false; - return false; - } - - // copy coarse path - coarse_path_ = path_; - - // Smooth the trajectory - pathSmooth(path_, collision_checker); - - // Ship it! - return is_trace_success; -} - -PointSet RRT_Planner::getPath() -{ - return path_; -} - -PointSet RRT_Planner::getCoarsePath() -{ - return coarse_path_; -} - -void RRT_Planner::interpolate(const ViewPoint &p1, const ViewPoint &p2, PointSet &path) -{ - // Interpolate points between two point -- exclusive of p1 and p2 - - ViewPoint check_point = p1; - ViewPoint direction = subtract(p2, p1); - double dist = norm(direction); - direction = divide(direction, dist); - ViewPoint step = multiply(direction, dense_step_); - while (norm(subtract(check_point, p1)) < dist - dense_step_) - { - check_point = addPoint(check_point, step); - path.push_back(check_point); - } -} - -// void RRT_Planner::pathSmooth(PointSet &path, collision_checker_ns::CollisionChecker &collision_checker) -// { -// /* Interpolate points --> smooth path */ -// // DEBUG -// // std::cout << "Smoothing Path ..."< check_idx) -// { -// forward_idx = int(fmin(forward_idx, num_path - 1)); -// p_forward = temp_path[forward_idx]; -// if (validEdge(p_forward, p_check, collision_checker)) -// { -// interpolate(p_check, p_forward, path); -// check_idx = forward_idx - 1; -// break; -// } -// forward_idx--; -// } -// last_p = temp_path[check_idx]; -// check_idx++; -// cur_p = temp_path[check_idx]; -// if (normSq(subtract(cur_p, last_p)) > kNorm_limit_sq_) -// { -// interpolate(last_p, cur_p, path); -// } -// } -// } - -void RRT_Planner::pathSmooth(PointSet &path, - collision_checker_ns::CollisionChecker &collision_checker) -{ - // straight line, no shortening - if (path.size() < 2) - return; - - // original discretized path (copy) - PointSet Pr; - Pr.push_back(path.front()); - for (size_t j = 0; j + 1 < path.size(); ++j) - { - if (equal(path[j], path[j+1])) continue; - interpolate(path[j], path[j+1], Pr); - Pr.push_back(path[j+1]); - } - - path.clear(); - - // Start with the first point - ViewPoint anchor = Pr.front(); - path.push_back(anchor); - - // Helpers to bridge ViewPoint <-> VDB world coords - auto fromVdb = [](const openvdb::Vec3d &v) -> ViewPoint - { - ViewPoint p; - p.x = v.x(); - p.y = v.y(); - p.z = v.z(); - return p; - }; - - // Tunables - const double step_delta = std::max(1e-3, static_cast(dense_step_)); // linear push step - const int max_pivot_steps = 5; // guard for very thick obstacles - const double normal_inflation = 1.0; // for estimateSurfaceNormalAtHit - - // Scan forward along Pr, only appending when blocked (or at the very end) - - for (size_t i = 1; i < Pr.size(); ++i) - { - // Try to see Pr[i] from current anchor - openvdb::Vec3d hit_world; - const bool blocked = collision_checker.collisionCheckRayStrict(anchor, Pr[i], hit_world); - - if (!blocked) - { - // Visible, connect only if it's the last point on path - if (i == Pr.size() - 1) - { - interpolate(anchor, Pr[i], path); - path.push_back(Pr[i]); - } - continue; - } - - // Blocked on Pr[i]: try to insert a first-hit pivot - bool pivot_inserted = false; - - // Estimate outward normal from occupancy around the hit - openvdb::Vec3d n_world; - bool have_normal = collision_checker.estimateSurfaceNormalAtHit(hit_world, - normal_inflation, - n_world); - if (have_normal) - { - // Linearly push along the normal until the local footprint is free - double delta = 0; - ViewPoint pivot; - - for (int step_count=0; step_count < max_pivot_steps; step_count++) - { - delta += step_delta; - pivot = fromVdb(hit_world + n_world * delta); - if (!collision_checker.collisionCheckInFreeSpace(pivot)) - { - if (validEdge(anchor, pivot, collision_checker) and - validEdge(pivot, Pr[i], collision_checker)) - { - pivot_inserted = true; - interpolate(anchor, pivot, path); - anchor = pivot; - // new anchor - path.push_back(anchor); - if (i == Pr.size() - 1) - { - interpolate(anchor, Pr[i], path); - path.push_back(Pr[i]); - } - break; - } - } - } - } - else - { - std::cout << "no normal at hit point " << hit_world.x() << ", " << hit_world.y() << ", " << hit_world.z() << '\n'; - pivot_inserted = false; - } - - // No pivot added, then fast forward to the last visible point on path - if (!pivot_inserted) - { - // Pivot failed (or became unnecessary) — append the last visible original waypoint - interpolate(anchor, Pr[i-1], path); - anchor = Pr[i-1]; - // new anchor - path.push_back(anchor); - if (i == Pr.size() - 1) - { - path.push_back(Pr[i]); - } - } - } -} - -// void RRT_Planner::updateGridPtr(const openvdb::BoolGrid::Ptr& grid) { -// grid_map_ = grid; -// } diff --git a/robot/ros_ws/src/global/planners/exploration/src/utils.cpp b/robot/ros_ws/src/global/planners/exploration/src/utils.cpp deleted file mode 100644 index 5d7768f51..000000000 --- a/robot/ros_ws/src/global/planners/exploration/src/utils.cpp +++ /dev/null @@ -1,772 +0,0 @@ -#include "utils/utils.hpp" -#include -#include -#include - -std_msgs::msg::ColorRGBA heightToColor(double norm_z) -{ - std_msgs::msg::ColorRGBA color; - color.a = 1.0; - - double h = norm_z * 0.8; - int i = static_cast(h * 6.0); - double f = (h * 6.0) - i; - double q = 1.0 - f; - - switch (i % 6) - { - case 0: - color.r = 1.0; - color.g = f; - color.b = 0.0; - break; - case 1: - color.r = q; - color.g = 1.0; - color.b = 0.0; - break; - case 2: - color.r = 0.0; - color.g = 1.0; - color.b = f; - break; - case 3: - color.r = 0.0; - color.g = q; - color.b = 1.0; - break; - case 4: - color.r = f; - color.g = 0.0; - color.b = 1.0; - break; - case 5: - color.r = 1.0; - color.g = 0.0; - color.b = q; - break; - default: - color.r = 1.0; - color.g = 0.5; - color.b = 0.5; - break; - } - return color; -} - -std_msgs::msg::ColorRGBA valueToRainbowColor(float value) -{ - std_msgs::msg::ColorRGBA color; - value = std::fmod(value, 1.0f); // Wrap values > 1.0 around - - float r = std::fabs(value * 6 - 3) - 1; - float g = 2 - std::fabs(value * 6 - 2); - float b = 2 - std::fabs(value * 6 - 4); - - color.r = std::clamp(r, 0.0f, 1.0f); - color.g = std::clamp(g, 0.0f, 1.0f); - color.b = std::clamp(b, 0.0f, 1.0f); - color.a = 1.0; - return color; -} - -// void generateVDBMarker(const GridT::Ptr& grid, -// const std::string& frame_id, -// visualization_msgs::msg::Marker& marker_msg) -// { -// marker_msg.header.frame_id = frame_id; -// marker_msg.header.stamp = rclcpp::Clock().now(); -// marker_msg.ns = "vdb_grid"; -// marker_msg.id = 0; -// marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; -// marker_msg.action = visualization_msgs::msg::Marker::ADD; -// marker_msg.pose.orientation.w = 1.0; -// marker_msg.color.a = 1.0; -// marker_msg.frame_locked = true; - -// double voxel_size = grid->voxelSize()[0]; -// marker_msg.scale.x = voxel_size; -// marker_msg.scale.y = voxel_size; -// marker_msg.scale.z = voxel_size; - -// openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); -// double z_min = grid->indexToWorld(bbox.min()).z(); -// double z_max = grid->indexToWorld(bbox.max()).z(); -// double dz = z_max - z_min; - -// for (auto iter = grid->cbeginValueOn(); iter; ++iter) { -// openvdb::Vec3d world = grid->indexToWorld(iter.getCoord()); - -// geometry_msgs::msg::Point pt; -// pt.x = world.x(); -// pt.y = world.y(); -// pt.z = world.z(); -// marker_msg.points.push_back(pt); - -// double norm_z = (dz > 1e-3) ? (world.z() - z_min) / dz : 0.5; -// marker_msg.colors.push_back(heightToColor(norm_z)); -// } - -// if (marker_msg.points.empty()) { -// marker_msg.action = visualization_msgs::msg::Marker::DELETE; -// } -// } - -void generateVDBMarker(const openvdb::FloatGrid::Ptr &grid, - const std::string &frame_id, - visualization_msgs::msg::Marker &marker_msg) -{ - marker_msg.header.frame_id = frame_id; - marker_msg.header.stamp = rclcpp::Clock().now(); - marker_msg.ns = "vdb_grid"; - marker_msg.id = 0; - marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; - marker_msg.action = visualization_msgs::msg::Marker::ADD; - marker_msg.pose.orientation.w = 1.0; - marker_msg.color.a = 1.0; - marker_msg.frame_locked = true; - - double voxel_size = grid->voxelSize()[0]; - marker_msg.scale.x = voxel_size; - marker_msg.scale.y = voxel_size; - marker_msg.scale.z = voxel_size; - - openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); - double z_min = grid->indexToWorld(bbox.min()).z(); - double z_max = grid->indexToWorld(bbox.max()).z(); - double dz = z_max - z_min; - - for (auto iter = grid->cbeginValueOn(); iter; ++iter) - { - if (iter.getValue() <= 0.0f) - { - continue; - } - - openvdb::Vec3d world = grid->indexToWorld(iter.getCoord()); - - geometry_msgs::msg::Point pt; - pt.x = world.x(); - pt.y = world.y(); - pt.z = world.z(); - marker_msg.points.push_back(pt); - - double norm_z = (dz > 1e-3) ? (world.z() - z_min) / dz : 0.5; - marker_msg.colors.push_back(heightToColor(norm_z)); - } - - if (marker_msg.points.empty()) - { - marker_msg.action = visualization_msgs::msg::Marker::DELETE; - } -} - -void generateFrontierMarker(const openvdb::BoolGrid::Ptr &grid, - const std::string &frame_id, - visualization_msgs::msg::Marker &marker_msg) -{ - marker_msg.header.frame_id = frame_id; - marker_msg.header.stamp = rclcpp::Clock().now(); - marker_msg.ns = "vdb_grid"; - marker_msg.id = 0; - marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; - marker_msg.action = visualization_msgs::msg::Marker::ADD; - marker_msg.pose.orientation.w = 1.0; - marker_msg.color.a = 1.0; - marker_msg.frame_locked = true; - - double voxel_size = grid->voxelSize()[0]; - marker_msg.scale.x = voxel_size; - marker_msg.scale.y = voxel_size; - marker_msg.scale.z = voxel_size; - - openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); - double z_min = grid->indexToWorld(bbox.min()).z(); - double z_max = grid->indexToWorld(bbox.max()).z(); - double dz = z_max - z_min; - - for (auto iter = grid->cbeginValueOn(); iter; ++iter) - { - if (!iter.getValue()) - { - continue; - } - - openvdb::Vec3d world = grid->indexToWorld(iter.getCoord()); - - geometry_msgs::msg::Point pt; - pt.x = world.x(); - pt.y = world.y(); - pt.z = world.z(); - - std_msgs::msg::ColorRGBA color; - color.r = 1.0; - color.g = 0.0; - color.b = 0.0; - color.a = 1.0; - - marker_msg.points.push_back(pt); - marker_msg.colors.push_back(color); - } - - if (marker_msg.points.empty()) - { - marker_msg.action = visualization_msgs::msg::Marker::DELETE; - } -} - -void generateClusterMarker(const openvdb::FloatGrid::Ptr &grid, - const std::string &frame_id, - visualization_msgs::msg::Marker &marker_msg) -{ - marker_msg.header.frame_id = frame_id; - marker_msg.header.stamp = rclcpp::Clock().now(); - marker_msg.ns = "cluster_grid"; - marker_msg.id = 0; - marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; - marker_msg.action = visualization_msgs::msg::Marker::ADD; - marker_msg.pose.orientation.w = 1.0; - marker_msg.color.a = 1.0; - marker_msg.frame_locked = true; - - double voxel_size = grid->voxelSize()[0]; - marker_msg.scale.x = voxel_size; - marker_msg.scale.y = voxel_size; - marker_msg.scale.z = voxel_size; - - for (auto iter = grid->cbeginValueOn(); iter; ++iter) - { - float cluster_val = iter.getValue(); - if (cluster_val <= 0.0f) - continue; // skip unclustered voxels - - openvdb::Vec3d world = grid->indexToWorld(iter.getCoord()); - - geometry_msgs::msg::Point pt; - pt.x = world.x(); - pt.y = world.y(); - pt.z = world.z(); - marker_msg.points.push_back(pt); - - marker_msg.colors.push_back(valueToRainbowColor(cluster_val)); - } - - if (marker_msg.points.empty()) - { - marker_msg.action = visualization_msgs::msg::Marker::DELETE; - } -} - -bool surface_edge_frontier(openvdb::FloatGrid::Accessor accessor, - openvdb::Coord ijk) -{ - openvdb::math::Coord ijk_n; - float gridvalue; - std::vector unknown_list; - std::vector occupied_list; - for (int di = -1; di <= 1; ++di) - { - ijk_n[0] = ijk[0] + di; - for (int dj = -1; dj <= 1; ++dj) - { - ijk_n[1] = ijk[1] + dj; - for (int dk = -1; dk <= 1; ++dk) - { - ijk_n[2] = ijk[2] + dk; - bool surround_state = accessor.probeValue(ijk_n, gridvalue); - // if there is unknown neighbor, surround_state will be inactive - if (!surround_state) - { - unknown_list.push_back(Eigen::Vector3d(di, dj, dk)); - // std::cout << "pushing unknown" << '\n'; - } - else if (gridvalue > 0) - { - occupied_list.push_back(Eigen::Vector3d(di, dj, dk)); - // std::cout << "pushing occ" << '\n'; - } - } - } - } - if (unknown_list.empty() || occupied_list.empty()) - { - return false; - } - for (size_t i = 0; i < unknown_list.size(); ++i) - { - for (size_t j = 0; j < occupied_list.size(); ++j) - { - Eigen::Vector3d un_occ_diff_vec = unknown_list[i] - occupied_list[j]; - // std::cout << "diff is " << un_occ_diff_vec.dot(un_occ_diff_vec) << '\n'; - if (un_occ_diff_vec.dot(un_occ_diff_vec) <= 3) - { - return true; - } - } - } - return false; -} - -void create_clustering_coordbbox(const openvdb::BoolGrid::Ptr grid, - openvdb::math::CoordBBox &coord_bbox_out, - const openvdb::Vec3d ¢er, - const float frontier_count_cube_dim) -{ - openvdb::Vec3d bbox_min, bbox_max; - openvdb::Coord ijk_bbox_min, ijk_bbox_max; - openvdb::math::Transform &grid_tf(grid->transform()); - - bbox_min.x() = center.x() - (frontier_count_cube_dim / 2); - bbox_max.x() = center.x() + (frontier_count_cube_dim / 2); - - bbox_min.y() = center.y() - (frontier_count_cube_dim / 2); - bbox_max.y() = center.y() + (frontier_count_cube_dim / 2); - - bbox_min.z() = center.z() - (frontier_count_cube_dim / 2); - bbox_max.z() = center.z() + (frontier_count_cube_dim / 2); - - ijk_bbox_min = grid_tf.worldToIndexCellCentered(bbox_min); - ijk_bbox_max = grid_tf.worldToIndexCellCentered(bbox_max); - - coord_bbox_out.reset(ijk_bbox_min, ijk_bbox_max); -} - -void point_clustering_vis(openvdb::BoolGrid::Ptr grid, - const float cluster_cube_dim, - pcl::PointCloud::Ptr cluster_cloud, - const int voxel_cluster_count_thresh, - std::vector> &clustered_points, - openvdb::FloatGrid::Ptr visualized_clusters) -{ - // for visualization only - float cluster_num = 0; - openvdb::FloatGrid::Accessor vis_acc = visualized_clusters->getAccessor(); - // - - openvdb::BoolGrid::Ptr grid_copy = grid->deepCopy(); - int voxel_count = 0; - openvdb::BoolGrid::Accessor acc = grid_copy->getAccessor(); - const openvdb::math::Transform &grid_tf(grid_copy->transform()); - pcl::PointXYZI cluster_score_point; - bool voxel_value = false; - std::vector points_buffer; - for (openvdb::BoolGrid::ValueOnCIter itr = grid_copy->cbeginValueOn(); itr; ++itr) - { - voxel_value = itr.getValue(); - // voxels that have not been "marked" before - if (voxel_value) - { - // for visualization only - std::vector vis_boxes; - // - - openvdb::Coord ijk = itr.getCoord(); - openvdb::Vec3d xyz = grid_tf.indexToWorld(ijk); - // create a bbox centered around xyz - openvdb::math::CoordBBox bbox; - create_clustering_coordbbox(grid_copy, bbox, xyz, cluster_cube_dim); - // iterate through bbox and find intersecting grid voxels - // save points to buffer, copy from buffer to output list - // if cluster is big enough - // count neighboring voxels (with voxel_value > voxel_mark_priority_medium) within bbox - voxel_count = 0; - points_buffer.clear(); - for (auto ijk = bbox.beginXYZ(); ijk; ++ijk) - { - openvdb::Coord idx = *ijk; - openvdb::Vec3d pt_xyz = grid_tf.indexToWorld(idx); - if (acc.getValue(idx)) - { - Eigen::Vector3d point{pt_xyz.x(), pt_xyz.y(), pt_xyz.z()}; - points_buffer.push_back(point); - voxel_count += 1; - - // for visualization only - openvdb::Coord vis_ijk = idx; - vis_boxes.push_back(vis_ijk); - // - } - } - // if nn vox count > thresh - // insert xyz and nn vox count as an XYZI point into cluster cloud - // mark all nn voxels and cluster centroid vox as --> voxel_mark_priority_medium - if (voxel_count >= voxel_cluster_count_thresh) - { - Eigen::Vector3d cluster_centroid = VDBUtil::computeCentroid(points_buffer); - cluster_score_point.x = cluster_centroid.x(); - cluster_score_point.y = cluster_centroid.y(); - cluster_score_point.z = cluster_centroid.z(); - cluster_score_point.intensity = voxel_count; - cluster_cloud->push_back(cluster_score_point); - mark_voxels_bool(points_buffer, grid_copy, false); - clustered_points.push_back(points_buffer); - - // for visualization only, one cluster one value, thus one color. - cluster_num += 0.1; - for (size_t pt = 0; pt < vis_boxes.size(); ++pt) - { - vis_acc.setValue(vis_boxes[pt], cluster_num); - } - // - } - } - } -} - -void mark_voxels_bool(const std::vector &point_list, - openvdb::BoolGrid::Ptr grid, - const bool voxel_value) -{ - openvdb::BoolGrid::Accessor acc = grid->getAccessor(); - const openvdb::math::Transform &grid_tf(grid->transform()); - for (auto &point : point_list) - { - openvdb::Vec3d xyz{point.x(), point.y(), point.z()}; - openvdb::Coord ijk = grid_tf.worldToIndexCellCentered(xyz); - acc.setValue(ijk, voxel_value); - } -} - -Eigen::Vector3d VDBUtil::computeCentroid(const std::vector &point_list) -{ - - Eigen::Vector3d sum = std::accumulate(point_list.begin(), - point_list.end(), - Eigen::Vector3d::Zero().eval()); - // Eigen::Vector3d centroid {sum / point_list.size()}; - - return sum / point_list.size(); -} - -void VDBUtil::updateOccMapFromNdArray(openvdb::FloatGrid::Ptr grid_logocc, - pcl::PointCloud::Ptr xyz, - const tf2::Vector3 &origin, - float l_free, - float l_occ, - float l_min, - float l_max, - int hit_thickness, - double threshold, - float visited_cleared_logocc_min_thresh) -{ - openvdb::FloatGrid::Accessor acc = grid_logocc->getAccessor(); - openvdb::Vec3d origin2(origin.x(), origin.y(), origin.z()); - openvdb::Vec3d origin_ijk = grid_logocc->worldToIndex(origin2); - - for (size_t i = 0; i < xyz->points.size(); ++i) - { - openvdb::Vec3d p_ijk = grid_logocc->worldToIndex(openvdb::Vec3d(xyz->points[i].x, - xyz->points[i].y, - xyz->points[i].z)); - - openvdb::Vec3d dir(p_ijk - origin_ijk); - double range = dir.length(); - dir.normalize(); - openvdb::math::Ray ray(origin_ijk, dir); - // ray, start_time, max_time - - if (range <= threshold) - { - openvdb::math::DDA, 0> dda(ray, 0., range); - // want to stop before dda.time == dda.maxTime - do - { - openvdb::Coord ijk(dda.voxel()); - float ll = acc.getValue(ijk); - if (ll < visited_cleared_logocc_min_thresh) - { - acc.setValue(ijk, std::max(l_min, ll + l_free)); - } - dda.step(); - } while (dda.time() < dda.maxTime()); - - // post condition, at hit voxel - for (int i = 0; i < hit_thickness; ++i) - { - openvdb::Coord ijk(dda.voxel()); - float ll = acc.getValue(ijk); - if (ll < visited_cleared_logocc_min_thresh) - { - acc.setValue(ijk, std::min(l_max, ll + l_occ)); - } - dda.step(); - } - } - else - { - openvdb::math::DDA, 0> dda(ray, 0., std::min(range, threshold)); - do - { - openvdb::Coord ijk(dda.voxel()); - float ll = acc.getValue(ijk); - if (ll < visited_cleared_logocc_min_thresh) - { - acc.setValue(ijk, std::max(l_min, ll + l_free)); - } - dda.step(); - } while (dda.time() < dda.maxTime()); - } - } -} - -void VDBUtil::setVoxelSize(openvdb::GridBase &grid, double vs) -{ - // cell-centered - const openvdb::math::Vec3d offset(vs / 2., vs / 2., vs / 2.); - openvdb::math::Transform::Ptr tf = openvdb::math::Transform::createLinearTransform(vs); - tf->postTranslate(offset); - grid.setTransform(tf); -} - -bool VDBUtil::collCheckPointInPartialFreeSpace(const openvdb::FloatGrid::Ptr grid, - const Eigen::Vector3d &point_xyz, - const float length, - const float breadth, - const float height, - const float l_occ, - const float unknown_fraction_thresh, - const float occupied_fraction_thresh) -{ - - bool point_is_in_free_space; - if (grid->empty()) - { - std::cerr << "[VDBUtil::checkPointInFreeSpace] Grid is empty. \ - Cannot perform collision check. Exiting." - << "\n"; - return false; - } - const openvdb::math::Transform &grid_tf(grid->transform()); - openvdb::Vec3d xyz(point_xyz.x(), point_xyz.y(), point_xyz.z()); - openvdb::math::CoordBBox coord_bbox; - - VDBUtil::createCoordBBox(grid, coord_bbox, xyz, - length, breadth, height); - - long int bbox_total_vox_count = 0; - long int unknown_vox_count = 0; - long int occupied_vox_count = 0; - openvdb::FloatGrid::Accessor acc = grid->getAccessor(); - for (auto ijk = coord_bbox.beginXYZ(); ijk; ++ijk) - { - ++bbox_total_vox_count; - openvdb::Coord idx = *ijk; - float vox_val = 0.0; - bool vox_state = acc.probeValue(idx, vox_val); - if (vox_state && vox_val > l_occ) - { - ++occupied_vox_count; - } - else if (!vox_state) - { - ++unknown_vox_count; - } - } - double occupied_fraction = double(occupied_vox_count) / double(bbox_total_vox_count); - double unknown_fraction = double(unknown_vox_count) / double(bbox_total_vox_count); - // if ( occupied_fraction <= occupied_fraction_thresh && unknown_fraction <= unknown_fraction_thresh) { - // point_is_in_free_space = true; - // } else { // in collision - // point_is_in_free_space = false; - // } - - if (occupied_fraction > occupied_fraction_thresh || unknown_fraction > unknown_fraction_thresh) - { - point_is_in_free_space = false; - } - else - { // in free space - point_is_in_free_space = true; - } - - return point_is_in_free_space; -} - -bool VDBUtil::checkCollisionAlongRay(const openvdb::FloatGrid::Ptr& grid, - const openvdb::Vec3d &p1_xyz, - const openvdb::Vec3d &p2_xyz, - const float bbox_length, - const float bbox_breadth, - const float bbox_height, - const float l_occ, - openvdb::Vec3d &hit_point, - float dist_offset_from_cluster, - bool consider_unknown_occupied) -{ - - bool in_collision = false; - openvdb::Coord ray_element_ijk; - openvdb::Vec3d ray_element_xyz; - - openvdb::FloatGrid::Accessor acc = grid->getAccessor(); - const openvdb::math::Transform &grid_tf(grid->transform()); - - openvdb::Vec3d p1_ijk = grid_tf.worldToIndex(p1_xyz); - openvdb::Vec3d p2_ijk = grid_tf.worldToIndex(p2_xyz); - openvdb::Vec3d dir(p2_ijk - p1_ijk); - double distance = dir.length(); - dir.normalize(); - openvdb::math::Ray ray(p1_ijk, dir); - openvdb::math::DDA, 0> dda(ray, 0., distance); - // want to stop before dda.time == dda.maxTime - // do { - while (dda.next() < dda.maxTime()) - { - - ray_element_ijk = dda.voxel(); - // check for robot model cube occupancy - ray_element_xyz = grid->indexToWorld(ray_element_ijk); - openvdb::math::CoordBBox coord_bbox; - VDBUtil::createCoordBBox(grid, coord_bbox, ray_element_xyz, - bbox_length, bbox_breadth, bbox_height); - if (VDBUtil::checkIntersectWithCoordBBox(coord_bbox, grid, - l_occ, consider_unknown_occupied)) - { - in_collision = true; - break; - } - dda.step(); - } - // } while (dda.time() < dda.maxTime()); - // } while (dda.next() < dda.maxTime()); - - if (in_collision) - { - // openvdb::Vec3d hit_ray(ray_element_ijk - p1_ijk); - // double hit_distance = hit_ray.length(); - // double min_threshold_distance = distance - dist_offset_from_cluster; - // if (hit_distance >= min_threshold_distance) - // { - // return false; - // } - // post condition, at hit voxel - hit_point = ray_element_xyz; - return true; - } - else - { - return false; - } -} - -long int VDBUtil::countVoxelsInsideBbox(const openvdb::FloatGrid::Ptr &grid, - const Eigen::Vector3d &bbox_center, - const float length, - const float breadth, - const float height) -{ - - openvdb::CoordBBox bbox; - long int vox_count = 0; - float log_occ_val = 0.0; - const openvdb::math::Transform &grid_tf(grid->transform()); - openvdb::FloatTree::Ptr grid_tree = grid->treePtr(); - createCoordBBox(grid, - bbox, - convertEigenVecToVec3D(bbox_center), - length, - breadth, - height); - - for (auto bbox_iter = bbox.beginXYZ(); bbox_iter; ++bbox_iter) - { - openvdb::Coord ijk = *bbox_iter; - if (grid_tree->probeValue(ijk, log_occ_val)) - { - vox_count += 1; - } - } - return vox_count; -} - -// void VDBUtil::createCoordBBox(const openvdb::FloatGrid::Ptr grid, -// openvdb::math::CoordBBox &coord_bbox_out, -// const openvdb::Vec3d ¢er, -// const float bbox_length, -// const float bbox_breadth, -// const float bbox_height) -// { - -// openvdb::Vec3d bbox_min, bbox_max; -// openvdb::Coord ijk_bbox_min, ijk_bbox_max; -// openvdb::math::Transform &grid_tf(grid->transform()); - -// bbox_min.x() = center.x() - (bbox_length / 2); -// bbox_max.x() = center.x() + (bbox_length / 2); - -// bbox_min.y() = center.y() - (bbox_breadth / 2); -// bbox_max.y() = center.y() + (bbox_breadth / 2); - -// bbox_min.z() = center.z() - (bbox_height / 2); -// bbox_max.z() = center.z() + (bbox_height / 2); - -// ijk_bbox_min = grid_tf.worldToIndexCellCentered(bbox_min); -// ijk_bbox_max = grid_tf.worldToIndexCellCentered(bbox_max); - -// coord_bbox_out.reset(ijk_bbox_min, ijk_bbox_max); -// } - -// The above implementation rounds to the nearest cell center. -// If bbox_max lands just past a cell center, rounding can pull it back and -// you’ll silently drop the outermost layer of cells. -// Use a continuous index bbox and floor/ceil: -void VDBUtil::createCoordBBox(const openvdb::FloatGrid::Ptr grid, - openvdb::math::CoordBBox &coord_bbox_out, - const openvdb::Vec3d ¢er, - const float bbox_length, - const float bbox_breadth, - const float bbox_height) -{ - const auto& tf = grid->transform(); - const openvdb::Vec3d half(bbox_length*0.5, bbox_breadth*0.5, bbox_height*0.5); - - // World-space box -> continuous index-space box - const openvdb::BBoxd wbox(center - half, center + half); - const openvdb::BBoxd ibox = tf.worldToIndex(wbox); - - // Integer (cell-centered) bounds: floor(min), ceil(max) - const openvdb::Coord ijk_min( - static_cast(std::floor(ibox.min().x() + 0.5)), - static_cast(std::floor(ibox.min().y() + 0.5)), - static_cast(std::floor(ibox.min().z() + 0.5))); - - const openvdb::Coord ijk_max( - static_cast(std::ceil (ibox.max().x() - 0.5)), - static_cast(std::ceil (ibox.max().y() - 0.5)), - static_cast(std::ceil (ibox.max().z() - 0.5))); - - coord_bbox_out.reset(ijk_min, ijk_max); -} - -bool VDBUtil::checkIntersectWithCoordBBox(const openvdb::math::CoordBBox &search_bbox, - const openvdb::FloatGrid::Ptr grid, - float occupancy_threshold, - bool consider_unknown_occupied) -{ - - openvdb::math::Transform &grid_tf(grid->transform()); - const size_t count = search_bbox.volume(); - openvdb::Vec3d xyz; - openvdb::FloatGrid::Accessor acc = grid->getAccessor(); - for (auto iter = search_bbox.beginXYZ(); iter; ++iter) - { - openvdb::Coord ijk = *iter; - float vox_val = 0.0; - bool vox_state = acc.probeValue(ijk, vox_val); - if (vox_state && vox_val > occupancy_threshold) - { - return true; - } - else if (consider_unknown_occupied && std::fabs(vox_val) <= FLT_EPSILON) - { - return true; - } - } - return false; -} - -openvdb::Vec3d VDBUtil::convertEigenVecToVec3D(const Eigen::Vector3d &point) -{ - - return openvdb::Vec3d(point.x(), point.y(), point.z()); -} \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/src/viewpoint_sampling.cpp b/robot/ros_ws/src/global/planners/exploration/src/viewpoint_sampling.cpp deleted file mode 100644 index 827325211..000000000 --- a/robot/ros_ws/src/global/planners/exploration/src/viewpoint_sampling.cpp +++ /dev/null @@ -1,488 +0,0 @@ -/* - * Viewpoint Sampling Module - * Copyright (C) 2020 Rohit Garg - All rights reserved - * Original author: rg1@cmu.edu / rohitgarg617@gmail.com - * Modified by junbiny@andrew.cmu.edu, removed matplotlib part and modified viewpoint sampling method - */ - -#include "../include/viewpoint_sampling.hpp" - -namespace viewpoint_sampling_ns -{ - // constructor - ViewpointSampling::ViewpointSampling(ListOfClustersT &cluster_list, - double viewing_distance, - float bbox_length, - float bbox_breadth, - float bbox_height, - float l_occ, - float vox_size, - uint16_t num_viewp_sample_per_cluster, - double viewing_distance_inner_r, - double viewing_distance_outer_r, - double viewing_distance_z_offset, - double coll_check_endpoint_offset, - double bbox_fraction_viewp_centroid_coll_check, - double viewp_bbox_unknown_frac_thresh, - double viewp_bbox_occ_frac_thresh) - { - - viewing_distance_ = viewing_distance; - bbox_length_ = bbox_length; - bbox_breadth_ = bbox_breadth; - bbox_height_ = bbox_height; - l_occ_ = l_occ; - vox_size_ = vox_size; - num_viewp_to_sample_ = num_viewp_sample_per_cluster; - viewing_distance_inner_r_ = viewing_distance_inner_r; - viewing_distance_outer_r_ = viewing_distance_outer_r; - viewing_distance_z_offset_ = viewing_distance_z_offset; - coll_check_endpoint_offset_ = coll_check_endpoint_offset; - bbox_fraction_viewp_centroid_coll_check_ = bbox_fraction_viewp_centroid_coll_check; - cluster_list_ = std::make_shared(cluster_list); - viewpoint_list_ = pcl::PointCloud::Ptr(new pcl::PointCloud()); - viewp_bbox_unknown_frac_thresh_ = viewp_bbox_unknown_frac_thresh; - viewp_bbox_occ_frac_thresh_ = viewp_bbox_occ_frac_thresh; - } - - // reset data stored by the class - void ViewpointSampling::reset(ListOfClustersT &cluster_list) - { - - // TODO -> not sure if this make_shared is required - cluster_list_ = std::make_shared(cluster_list); - xy_points_.clear(); - viewpoint_list_->clear(); - } - - // update the global map grid pointer - void ViewpointSampling::updateGridPtr(const openvdb::FloatGrid::Ptr &grid) - { - grid_map_ = grid; - } - - // update the cluster centroid list pointer - void ViewpointSampling::updateCentroidListPtr( - const pcl::PointCloud::Ptr ¢roid_list) - { - centroid_list_ = centroid_list; - } - - // main method that samples viewpoints - void ViewpointSampling::sampleViewpoints() - { - cluster_idx_ = -1; - - for (const auto &cluster : *cluster_list_) - { - - cluster_idx_ += 1; - - // current_cluster_centroid_ = computeMean(); - pcl::PointXYZI centroid = centroid_list_->at(cluster_idx_); - current_cluster_score_ = centroid.intensity; - current_cluster_centroid_.x() = centroid.x; - current_cluster_centroid_.y() = centroid.y; - current_cluster_centroid_.z() = centroid.z; - sampleViewpointsAroundCentroid(); - } - } - - // remove z component from 3D points - void ViewpointSampling::projectToXYPlane() - { - xy_points_.clear(); - for (const auto &point : cluster_list_->at(cluster_idx_)) - { - Eigen::Vector2d point_in{point.x(), point.y()}; - xy_points_.push_back(point_in); - } - } - - // logging to file - void ViewpointSampling::logDataToFile() - { - - std::string file1 = "cluster"; - std::string file_path1 = setupFilePath(cluster_idx_, file1); - writeVecToCSVfile(file_path1, xy_points_); - - std::string file2 = "fit_line_wsolver" + std::to_string(solver_type_); - std::string file_path2 = setupFilePath(cluster_idx_, file2); - Eigen::MatrixXd output_mat = fitted_line_; - writeToCSVfile(file_path2, output_mat); - } - - // generate file path - std::string ViewpointSampling::setupFilePath(int file_index, std::string file_name) - { - std::string fname = file_name + "_"; - std::string path = "/home/rohit/rWorkspace/vdb_map_ws/src/map_processor/logs/"; - std::string idx = std::to_string(file_index); - std::string fformat = ".txt"; - return path + fname + idx + fformat; - } - - // write matrix to a CSV file - void ViewpointSampling::writeToCSVfile(std::string file_name, Eigen::MatrixXd &matrix) - { - const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n"); - std::ofstream file(file_name.c_str()); - file << matrix.format(CSVFormat); - file.close(); - } - - // write vector to a CSV file - void ViewpointSampling::writeVecToCSVfile(std::string file_name, - std::vector &points) - { - const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, 0, ", ", "\n"); - std::ofstream file(file_name.c_str()); - for (const auto &point : points) - { - file << point(0) << ", " << point(1) << "\n"; - } - file.close(); - } - - // splitting into two separate vectors - void ViewpointSampling::toXVecYVec() - { - for (auto point : xy_points_) - { - cluster_x_points_.push_back(point.x()); - cluster_y_points_.push_back(point.y()); - } - } - - // discretize line into a list of points - void ViewpointSampling::drawPointsAlongLine(float spacing, float length) - { - double start_x = current_cluster_centroid_.x(); - uint64_t n_points = std::floor(length / spacing); - for (int i = 0; i < n_points; ++i) - { - double line_x_pt = start_x + (i * spacing); - double line_y_pt = (fitted_line_(0) * line_x_pt) + fitted_line_(0); - line_x_points_.push_back(line_x_pt); - line_y_points_.push_back(line_y_pt); - } - } - - // Ax=b -> A matrix - void ViewpointSampling::createAMatrix() - { - A_mat_ = Eigen::MatrixXd::Ones(xy_points_.size(), 2); - uint32_t idx = 0; - for (const auto &point : xy_points_) - { - Eigen::Vector2d point_in{point.x(), 1}; - A_mat_.row(idx) = point_in; - idx += 1; - } - } - - // Ax=b -> b matrix - void ViewpointSampling::createBMatrix() - { - b_vec_ = Eigen::VectorXd::Zero(xy_points_.size()); - uint32_t idx = 0; - for (const auto &point : xy_points_) - { - b_vec_(idx) = point.y(); - idx += 1; - } - } - - // for getting the cluster centroid - Eigen::Vector3d ViewpointSampling::computeMean() - { - - double sum_x = 0, sum_y = 0, sum_z = 0; - for (const auto &point : cluster_list_->at(cluster_idx_)) - { - sum_x += point.x(); - sum_y += point.y(); - sum_z += point.z(); - } - uint32_t n_points = cluster_list_->at(cluster_idx_).size(); - Eigen::Vector3d point_out{sum_x / n_points, sum_y / n_points, sum_z / n_points}; - return point_out; - } - - // get two points at a perpendicular distance from a line and a point on the line - void ViewpointSampling::computePointsPerpToLine() - { - - opposing_viewpoints_ = Eigen::MatrixXd::Zero(2, 3); - - double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0; - - double x_c = current_cluster_centroid_.x(); // cluster xy coords - double y_c = current_cluster_centroid_.y(); - - double m = fitted_line_(0); // slope of the line - double r = viewing_distance_; // distance from the line - - // ref: http://www.rasmus.is/uk/t/F/Su58k05.htm#:~:text=To%20find%20a%20direction%20vector,%2B%20by%20%2B%20c%20%3D%200. - // ref: https://math.stackexchange.com/questions/2043054/find-a-point-on-a-perpendicular-line-a-given-distance-from-another-point - opposing_viewpoints_(0, 0) = x_c + - std::sqrt(std::pow(r, 2) / (1 + (1 / std::pow(m, 2)))); - - opposing_viewpoints_(1, 0) = x_c - - std::sqrt(std::pow(r, 2) / (1 + (1 / std::pow(m, 2)))); - - opposing_viewpoints_(0, 1) = y_c - - (1 / m) * std::sqrt(std::pow(r, 2) / (1 + (1 / std::pow(m, 2)))); - - opposing_viewpoints_(1, 1) = y_c + - (1 / m) * std::sqrt(std::pow(r, 2) / (1 + (1 / std::pow(m, 2)))); - - // z -> same height as the cluster centroid since the - // view plane is perpendicular to XY plane - opposing_viewpoints_(0, 2) = current_cluster_centroid_.z(); - opposing_viewpoints_(1, 2) = current_cluster_centroid_.z(); - } - - // select one out of the two points on either side of the line - void ViewpointSampling::selectOnePoint() - { - - long int active_vox_count_side1 = 0, active_vox_count_side2 = 0; - - active_vox_count_side1 = VDBUtil::countVoxelsInsideBbox(grid_map_, - opposing_viewpoints_.row(0), - bbox_length_, - bbox_breadth_, - bbox_height_); - - active_vox_count_side2 = VDBUtil::countVoxelsInsideBbox(grid_map_, - opposing_viewpoints_.row(1), - bbox_length_, - bbox_breadth_, - bbox_height_); - - // viewpoint selection is happening using the number of active voxels - // found in a bbox centered around the opposing points - // whichever bbox has a higher count is chosen - if (active_vox_count_side1 >= active_vox_count_side2) - { - selected_viewpoint_ = opposing_viewpoints_.row(0); - } - else - { - selected_viewpoint_ = opposing_viewpoints_.row(1); - } - direction_vector_ = current_cluster_centroid_ - selected_viewpoint_; - } - - // assuming x-forward, z-up convention - float ViewpointSampling::computeYawFromDirVector() - { - return std::atan2(direction_vector_(1), direction_vector_(0)); - } - - pcl::PointXYZHSV ViewpointSampling::constructViewpoint() - { - pcl::PointXYZHSV view_point; - - view_point.x = selected_viewpoint_.x(); - view_point.y = selected_viewpoint_.y(); - view_point.z = selected_viewpoint_.z(); - view_point.h = heading_; - view_point.s = current_cluster_score_; - view_point.v = 0.0; - return view_point; - } - - // line fit using linear least squares - bool ViewpointSampling::fitLineToPoints(int solver_flag) - { - - createAMatrix(); - createBMatrix(); - // ref: https://eigen.tuxfamily.org/dox/group__LeastSquares.html - switch (solver_flag) - { - case 1: - useSVDDecomposition(); - break; - case 2: - useQRDecomposition(); - break; - case 3: - useNormalEquations(); - break; - } - return true; - } - - // SVD is the most accurate but slowest - void ViewpointSampling::useSVDDecomposition() - { - fitted_line_ = Eigen::Vector2d::Zero(); - fitted_line_ = A_mat_.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b_vec_); - } - - // QR decomposition sits between SVD and normal equations - void ViewpointSampling::useQRDecomposition() - { - fitted_line_ = Eigen::Vector2d::Zero(); - fitted_line_ = A_mat_.colPivHouseholderQr().solve(b_vec_); - } - - // solving A^T A x = A^T b (linear least squares using normal equations) - // fastest but also least accurate - void ViewpointSampling::useNormalEquations() - { - // Normal Equations method - fitted_line_ = Eigen::Vector2d::Zero(); - fitted_line_ = (A_mat_.transpose() * A_mat_).ldlt().solve(A_mat_.transpose() * b_vec_); - } - - // create viewpoints on a circle centered around the centroid - void ViewpointSampling::sampleViewpointsAroundCentroid() - { - - /* viewing_distance_inner_r_ = viewing_distance_inner_r; - viewing_distance_outer_r_ = viewing_distance_outer_r; - viewing_distance_z_offset_ */ - - // successively try viewpoints on different horizontal circles until some - // are successfully found - - double angle_spacing = num_viewp_to_sample_ / (2 * M_PI); - int count_viewpoints_added = 0; - - // First, try outer circle with 0 z offset - count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( - viewing_distance_outer_r_, - 0.0, - angle_spacing); - - if (count_viewpoints_added > 1) - { - // Don't add any more - return; - } - - // Next, try inner circle with 0 z offset - count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( - viewing_distance_inner_r_, - 0.0, - angle_spacing); - - if (count_viewpoints_added > 0) - { - // Don't add any more - return; - } - - // Try outer circle with +/- z offset - count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( - viewing_distance_outer_r_, - +viewing_distance_z_offset_, - angle_spacing); - count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( - viewing_distance_outer_r_, - -viewing_distance_z_offset_, - angle_spacing); - - if (count_viewpoints_added > 0) - { - // Don't add any more - return; - } - - // Try inner circle with +/- z offset - count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( - viewing_distance_inner_r_, - +viewing_distance_z_offset_, - angle_spacing); - count_viewpoints_added += sampleViewpointsAroundCentroidParamaterized( - viewing_distance_inner_r_, - -viewing_distance_z_offset_, - angle_spacing); - - if (count_viewpoints_added > 0) - { - // Don't add any more - return; - } - } - - // create viewpoints on a circle centered around the centroid - // with preset parameters - int ViewpointSampling::sampleViewpointsAroundCentroidParamaterized( - const double viewing_distance, - const double z_offset, - const double angle_spacing) - { - // return the number of viewpoints added - - int count_viewpoints_added = 0; - double psi = 0.0; - - for (int i = 0; i < num_viewp_to_sample_; ++i) - { - - psi += angle_spacing * i; - - Eigen::Vector3d viewpoint; - viewpoint.x() = current_cluster_centroid_.x() + (viewing_distance * std::cos(psi)); - viewpoint.y() = current_cluster_centroid_.y() + (viewing_distance * std::sin(psi)); - viewpoint.z() = current_cluster_centroid_.z() + z_offset; - - // bool viewpoint_is_in_free_space = VDBUtil::checkPointInFreeSpace(grid_map_, - // viewpoint, bbox_length_, - // bbox_breadth_, bbox_height_, - // l_occ_, true); - - bool viewpoint_is_in_free_space = - VDBUtil::collCheckPointInPartialFreeSpace(grid_map_, - viewpoint, bbox_length_, - bbox_breadth_, bbox_height_, - l_occ_, - viewp_bbox_unknown_frac_thresh_, - viewp_bbox_occ_frac_thresh_); - - openvdb::Vec3d hp; - openvdb::Vec3d viewpoint_vbd(viewpoint.x(), viewpoint.y(), viewpoint.z()); - openvdb::Vec3d centroid_vdb(current_cluster_centroid_.x(), - current_cluster_centroid_.y(), - current_cluster_centroid_.z()); - - bool consider_unknown_occupied = false; - - double ray_coll_check_bbox_length = - bbox_fraction_viewp_centroid_coll_check_ * bbox_length_; - double ray_coll_check_bbox_breadth = - bbox_fraction_viewp_centroid_coll_check_ * bbox_breadth_; - double ray_coll_check_bbox_height = - bbox_fraction_viewp_centroid_coll_check_ * bbox_height_; - - bool viewpoint_to_centroid_is_free_space = !VDBUtil::checkCollisionAlongRay( - grid_map_, - viewpoint_vbd, - centroid_vdb, - ray_coll_check_bbox_length, - ray_coll_check_bbox_breadth, - ray_coll_check_bbox_height, - l_occ_, - hp, - coll_check_endpoint_offset_, - consider_unknown_occupied); - - if (viewpoint_is_in_free_space && viewpoint_to_centroid_is_free_space) - { - selected_viewpoint_ = viewpoint; - direction_vector_ = current_cluster_centroid_ - selected_viewpoint_; - heading_ = computeYawFromDirVector(); - viewpoint_list_->push_back(constructViewpoint()); - count_viewpoints_added++; - } - } - return count_viewpoints_added; - } - -} // namespace viewpoint_sampling_ns \ No newline at end of file diff --git a/robot/ros_ws/src/global/planners/exploration/src/waypoint_routing_node.cpp b/robot/ros_ws/src/global/planners/exploration/src/waypoint_routing_node.cpp deleted file mode 100644 index c83c39878..000000000 --- a/robot/ros_ws/src/global/planners/exploration/src/waypoint_routing_node.cpp +++ /dev/null @@ -1,351 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "../include/exploration_node.hpp" -#include "../include/exploration_logic.hpp" - -#include -#include -#include - -#include "utils/utils.hpp" - -class WaypointRoutingNode : public ExplorationNode -{ - using ExplorationNode::ExplorationNode; -public: - void initialize() override; - -private: - std::string sub_target_path_topic_; - std::deque target_path; - - rclcpp::Subscription::SharedPtr sub_target_path; - void targetpathCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg); - - void timerCallback() override; - void generate_plan() override; -}; - -void WaypointRoutingNode::initialize() -{ - this->declare_parameter("sub_target_path_topic"); - if (!this->get_parameter("sub_target_path_topic", this->sub_target_path_topic_)) - { - RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_target_path_topic"); - return; - } - - // TF buffer and listener - tf_buffer = std::make_shared(this->get_clock()); - tf_listener = std::make_shared(*tf_buffer); - - this->sub_lidar = this->create_subscription( - sub_lidar_topic_, rclcpp::QoS(1).durability_volatile().best_effort(), std::bind(&WaypointRoutingNode::lidarCallback, this, std::placeholders::_1)); - this->sub_target_path = this->create_subscription( - sub_target_path_topic_, rclcpp::QoS(10), std::bind(&WaypointRoutingNode::targetpathCallback, this, std::placeholders::_1)); - - this->pub_global_plan = this->create_publisher(pub_global_plan_topic_, 10); - this->pub_goal_point = - this->create_publisher(pub_goal_point_viz_topic_, 10); - this->pub_trajectory_lines = - this->create_publisher(pub_trajectory_viz_topic_, 10); - this->pub_vdb = - this->create_publisher(pub_grid_viz_topic_, 10); - - // Set up the timer - this->timer = this->create_wall_timer(std::chrono::seconds(5), - std::bind(&WaypointRoutingNode::timerCallback, this)); - // Set up the service - this->srv_exploration_toggle = this->create_service( - srv_exploration_toggle_topic_, std::bind(&WaypointRoutingNode::ExplorationToggleCallback, this, - std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(this->get_logger(), "Exploration node initialized"); -} - -void WaypointRoutingNode::targetpathCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg) -{ - RCLCPP_INFO(this->get_logger(), "Received target path with %zu poses", msg->poses.size()); - this->target_path.assign(msg->poses.begin(), msg->poses.end()); -} - -void WaypointRoutingNode::generate_plan() -{ - RCLCPP_INFO(this->get_logger(), "Starting to generate plan..."); - - // std::tuple start_loc; - // if (this->generated_paths.size() == 0) - // { - // geometry_msgs::msg::Quaternion orientation = this->current_location.rotation; - // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); - // q.normalize(); - // double roll, pitch, yaw; - // tf2::Matrix3x3 m(q); - // m.getRPY(roll, pitch, yaw); - // start_loc = std::make_tuple(this->current_location.translation.x, - // this->current_location.translation.y, - // this->current_location.translation.z, yaw); - // } - // else - // { - // geometry_msgs::msg::Quaternion orientation = - // this->generated_paths.back().poses.back().pose.orientation; - // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); - // q.normalize(); - // double roll, pitch, yaw; - // tf2::Matrix3x3 m(q); - // m.getRPY(roll, pitch, yaw); - // start_loc = std::make_tuple(this->generated_paths.back().poses.back().pose.position.x, - // this->generated_paths.back().poses.back().pose.position.y, - // this->generated_paths.back().poses.back().pose.position.z, yaw); - // } - - ViewPoint start_point; - - if (this->generated_paths.empty() || this->generated_paths.back().poses.empty()) - { - // Case 1: no generated path yet → use current_location - geometry_msgs::msg::Quaternion orientation = this->current_location.rotation; - - tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); - q.normalize(); - - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - - start_point.x = this->current_location.translation.x; - start_point.y = this->current_location.translation.y; - start_point.z = this->current_location.translation.z; - - start_point.orientation = orientation; - start_point.orientation_yaw = yaw; - start_point.orientation_set = true; - } - else - { - // Case 2: use last pose of last generated path - const auto &last_pose = this->generated_paths.back().poses.back().pose; - geometry_msgs::msg::Quaternion orientation = last_pose.orientation; - - tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); - q.normalize(); - - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - - start_point.x = last_pose.position.x; - start_point.y = last_pose.position.y; - start_point.z = last_pose.position.z; - - start_point.orientation = orientation; - start_point.orientation_yaw = yaw; - start_point.orientation_set = true; - } - - // convert first goal waypoint to ViewPoint - ViewPoint goal_viewpoint; - - const auto &first_pose = target_path.front(); - - goal_viewpoint.x = first_pose.position.x; - goal_viewpoint.y = first_pose.position.y; - goal_viewpoint.z = first_pose.position.z; - - // Keep quaternion + yaw consistent - tf2::Quaternion q(first_pose.orientation.x, - first_pose.orientation.y, - first_pose.orientation.z, - first_pose.orientation.w); - q.normalize(); - - double roll, pitch, yaw; - tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); - - goal_viewpoint.orientation = first_pose.orientation; - goal_viewpoint.orientation_yaw = yaw; - goal_viewpoint.orientation_set = true; - - float timeout_duration = 5.0; - std::optional gen_path_opt = - // this->exploration_planner->generate_straight_rand_path(start_loc, timeout_duration); - // this->exploration_planner->select_viewpoint_and_plan(start_point, timeout_duration); - this->exploration_planner->plan_to_given_waypoint(start_point, goal_viewpoint); - if (gen_path_opt.has_value() && gen_path_opt.value().size() > 0) - { - RCLCPP_INFO(this->get_logger(), "Generated path with %ld points", - gen_path_opt.value().size()); - - // set the current goal location - this->current_goal_location = geometry_msgs::msg::Transform(); - this->current_goal_location.translation.x = std::get<0>(gen_path_opt.value().back()); - this->current_goal_location.translation.y = std::get<1>(gen_path_opt.value().back()); - this->current_goal_location.translation.z = std::get<2>(gen_path_opt.value().back()); - float z_rot = std::get<3>(gen_path_opt.value().back()); - tf2::Quaternion q; - q.setRPY(0, 0, z_rot); // Roll = 0, Pitch = 0, Yaw = yaw - q.normalize(); - this->current_goal_location.rotation.x = q.x(); - this->current_goal_location.rotation.y = q.y(); - this->current_goal_location.rotation.z = q.z(); - this->current_goal_location.rotation.w = q.w(); - - // publish the path - nav_msgs::msg::Path generated_single_path; - generated_single_path = nav_msgs::msg::Path(); - generated_single_path.header.stamp = this->now(); - generated_single_path.header.frame_id = world_frame_id_; - for (auto point : gen_path_opt.value()) - { - geometry_msgs::msg::PoseStamped point_msg; - point_msg.pose.position.x = std::get<0>(point); - point_msg.pose.position.y = std::get<1>(point); - point_msg.pose.position.z = std::get<2>(point); - // convert yaw rotation to quaternion - tf2::Quaternion q; - q.setRPY(0, 0, std::get<3>(point)); // Roll = 0, Pitch = 0, Yaw = yaw - q.normalize(); - point_msg.pose.orientation.x = q.x(); - point_msg.pose.orientation.y = q.y(); - point_msg.pose.orientation.z = q.z(); - point_msg.pose.orientation.w = q.w(); - point_msg.header.stamp = this->now(); - generated_single_path.poses.push_back(point_msg); - } - geometry_msgs::msg::PoseStamped last_goal_loc = generated_single_path.poses.back(); - this->current_goal_location.translation.x = last_goal_loc.pose.position.x; - this->current_goal_location.translation.y = last_goal_loc.pose.position.y; - this->current_goal_location.translation.z = last_goal_loc.pose.position.z; - this->current_goal_location.rotation.z = last_goal_loc.pose.orientation.z; - this->generated_paths.push_back(generated_single_path); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to generate path, size was 0"); - } -} - -void WaypointRoutingNode::timerCallback() -{ - RCLCPP_INFO(this->get_logger(), "timer callback start"); - // get current TF to world - try - { - geometry_msgs::msg::TransformStamped transform_stamped = - this->tf_buffer->lookupTransform(this->world_frame_id_, this->robot_frame_id_, - rclcpp::Time(0)); - this->current_location = transform_stamped.transform; - if (!this->received_first_robot_tf) - { - this->received_first_robot_tf = true; - this->last_location = this->current_location; - this->last_position_change = this->now(); - RCLCPP_WARN(this->get_logger(), "Received first robot_tf"); - } - } - catch (tf2::TransformException &ex) - { - RCLCPP_ERROR(this->get_logger(), "Robot tf not received: %s", ex.what()); - } - - - // visualize - visualization_msgs::msg::Marker vdb_marker_msg; - generateVDBMarker(this->exploration_planner->occ_grid, map_frame_id_, vdb_marker_msg); - vdb_marker_msg.header.stamp = this->now(); - this->pub_vdb->publish(vdb_marker_msg); - - if (this->enable_exploration) - { - if (!this->is_path_executing) - { - if (!target_path.empty() && this->received_first_robot_tf) - { - - this->generate_plan(); - - this->publish_plan(); - this->is_path_executing = true; - this->last_position_change = this->now(); // Reset stall timer when starting new plan - } - else - { - RCLCPP_WARN(this->get_logger(), "target path size is %zu", target_path.size()); - RCLCPP_WARN(this->get_logger(), "tf received is %d", this->received_first_robot_tf); - } - } - else - { - // check if the robot has reached the goal point - std::tuple current_point = std::make_tuple( - this->current_location.translation.x, this->current_location.translation.y, - this->current_location.translation.z); - std::tuple goal_point = std::make_tuple( - this->current_goal_location.translation.x, this->current_goal_location.translation.y, - this->current_goal_location.translation.z); - if (get_point_distance(current_point, goal_point) < - this->exploration_planner->path_end_threshold_m) - { - this->is_path_executing = false; - this->generated_paths.clear(); - this->target_path.pop_front(); - RCLCPP_INFO(this->get_logger(), "Reached goal point"); - } - else - { - // Check if position has changed significantly - std::tuple last_point = std::make_tuple(this->last_location.translation.x, - this->last_location.translation.y, - this->last_location.translation.z); - - if (get_point_distance(current_point, last_point) > this->position_change_threshold) - { - this->last_location = this->current_location; - this->last_position_change = this->now(); - } - else - { - // Check if we've been stationary for too long - rclcpp::Duration stall_duration = this->now() - this->last_position_change; - if (stall_duration.seconds() > this->stall_timeout_seconds) - { - RCLCPP_INFO(this->get_logger(), "Robot stationary for %f seconds, clearing plan", - stall_duration.seconds()); - this->is_path_executing = false; - this->generated_paths.clear(); - this->target_path.pop_front(); - this->last_position_change = this->now(); // Reset timer to avoid spam - } - } - } - } - } - - RCLCPP_INFO(this->get_logger(), "timer callback end"); -} - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->initialize(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml index 03d14ed67..b74ef802e 100644 --- a/simulation/isaac-sim/docker/docker-compose.yaml +++ b/simulation/isaac-sim/docker/docker-compose.yaml @@ -65,6 +65,11 @@ services: # offset in meters. - ISAAC_SIM_FOLLOW_CAM=${ISAAC_SIM_FOLLOW_CAM:-} - ISAAC_SIM_FOLLOW_CAM_OFFSET=${ISAAC_SIM_FOLLOW_CAM_OFFSET:-} + # Optional raw sensor_msgs/Image feed of the follow camera (topic name, + # published on the followed drone's ROS domain) and its "WxH" resolution + # — a third-person video source for headless runs. + - ISAAC_SIM_FOLLOW_CAM_TOPIC=${ISAAC_SIM_FOLLOW_CAM_TOPIC:-} + - ISAAC_SIM_FOLLOW_CAM_RES=${ISAAC_SIM_FOLLOW_CAM_RES:-} # Spawn-row center "x,y" in meters (default origin) — for scenes whose # origin is cluttered; see launch_scripts/pegasus_app.py. - ISAAC_SIM_SPAWN_XY=${ISAAC_SIM_SPAWN_XY:-} diff --git a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py index c59fcbbe8..974860861 100755 --- a/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py +++ b/simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py @@ -11,7 +11,7 @@ (pass ``save_scene_to=`` below) Env (see pegasus_app.py): ISAAC_SIM_LIVESTREAM, ISAAC_SIM_HEADLESS, -PLAY_SIM_ON_START, and ISAAC_SIM_SCENE / ISAAC_SIM_STAGE_SCALE (set by +PLAY_SIM_ON_START, ISAAC_SIM_SPAWN_XY, and ISAAC_SIM_SCENE / ISAAC_SIM_STAGE_SCALE (set by `airstack up --scene ` — a Pegasus catalog key or USD URL; default: Default Environment). """ @@ -26,21 +26,29 @@ simulation_app = create_simulation_app() from pegasus.simulator.params import SIMULATION_ENVIRONMENTS # noqa: E402 -from pegasus_app import PegasusApp, resolve_scene_from_env # noqa: E402 +from pegasus_app import ( # noqa: E402 + PegasusApp, + resolve_scene_from_env, + resolve_spawn_center_from_env, +) def main(): # Scene from `airstack up --scene` (or set ISAAC_SIM_SCENE to any # catalog key / USD URL); stage_scale converts cm-authored stages to m. env_url, stage_scale = resolve_scene_from_env(SIMULATION_ENVIRONMENTS) - print(f"[example_one] Scene: {env_url} (stage_scale={stage_scale})") + # ISAAC_SIM_SPAWN_XY="x,y" (meters) moves the single drone off the world + # origin — several imported stages are enclosed or unlit there. + spawn_x, spawn_y = resolve_spawn_center_from_env() + print(f"[example_one] Scene: {env_url} (stage_scale={stage_scale}) " + f"spawn=({spawn_x}, {spawn_y})") PegasusApp( env_url=env_url, stage_scale=stage_scale, drone_configs=[ { "domain_id": 1, # MAVLink port = 14540 + vehicle_id (= domain_id) - "x_m": 0.0, "y_m": 0.0, "z_m": 0.07, + "x_m": spawn_x, "y_m": spawn_y, "z_m": 0.07, # Single-drone scenes keep the historical prim/node names. "prim": "/World/base_link", "node_name": "PX4Multirotor", diff --git a/simulation/isaac-sim/launch_scripts/pegasus_app.py b/simulation/isaac-sim/launch_scripts/pegasus_app.py index 62373e8ce..ea56b3063 100644 --- a/simulation/isaac-sim/launch_scripts/pegasus_app.py +++ b/simulation/isaac-sim/launch_scripts/pegasus_app.py @@ -614,6 +614,29 @@ def _setup_follow_cam(self, stage): except Exception as exc: # headless variants may have no viewport carb.log_warn(f"Could not switch viewport to follow cam: {exc}") + # ISAAC_SIM_FOLLOW_CAM_TOPIC=: also publish the follow cam as a + # raw sensor_msgs/Image on the followed drone's ROS domain, so a + # headless run (CI, video capture without a display) still yields a + # third-person feed. ISAAC_SIM_FOLLOW_CAM_RES="WxH" (default 1280x720). + topic = os.environ.get("ISAAC_SIM_FOLLOW_CAM_TOPIC", "").strip() + if topic: + width, height = 1280, 720 + res_raw = os.environ.get("ISAAC_SIM_FOLLOW_CAM_RES", "").strip().lower() + if res_raw: + try: + width, height = (int(v) for v in res_raw.split("x")) + except ValueError: + carb.log_warn(f"ISAAC_SIM_FOLLOW_CAM_RES='{res_raw}' is not WxH — using 1280x720.") + try: + from scene_prep import add_camera_image_publisher + + add_camera_image_publisher( + "/World/FollowCamPublisher", FOLLOW_CAM_PATH, topic, + width=width, height=height, frame_id="follow_cam", domain_id=i, + ) + except Exception as exc: + carb.log_error(f"[follow_cam] could not wire the ROS image publisher: {exc}") + def _author_follow_cam(self): from isaacsim.core.utils.viewports import set_camera_view diff --git a/simulation/isaac-sim/utils/scene_prep.py b/simulation/isaac-sim/utils/scene_prep.py index de2ea11d5..61ac7a392 100644 --- a/simulation/isaac-sim/utils/scene_prep.py +++ b/simulation/isaac-sim/utils/scene_prep.py @@ -413,6 +413,72 @@ def add_overhead_camera_publisher(parent_graph_path: str, f"domain_id={domain_id}") +def add_camera_image_publisher(graph_path: str, + camera_prim_path: str, + topic: str, + width: int = 1280, + height: int = 720, + frame_id: str = "follow_cam", + domain_id: int = 1): + """Publish an existing USD camera as a raw ``sensor_msgs/Image`` stream. + + Builds a standalone OmniGraph at ``graph_path`` (own ROS2Context on + ``domain_id``; OnPlaybackTick → IsaacCreateRenderProduct → + ROS2CameraHelper). Used by pegasus_app's follow camera so a headless + Isaac Sim (no viewport window) still yields a third-person video feed + that ``ros2 bag`` / a recorder node can capture — CI runners and demo + captures alike. + """ + import omni.graph.core as og # lazy so non-sim contexts can import scene_prep + + controller = og.Controller() + g = graph_path + if og.get_graph_by_path(g) is None: + og.Controller.create_graph({"graph_path": g, "evaluator_name": "execution"}) + + nodes = { + "context": f"{g}/ROS2Context", + "playback": f"{g}/OnPlaybackTick", + "create_rp": f"{g}/CreateRenderProduct", + "rgb": f"{g}/RGBHelper", + "frame": f"{g}/FrameId", + "topic": f"{g}/Topic", + } + controller.edit( + graph_id=g, + edit_commands={ + og.Controller.Keys.CREATE_NODES: [ + (nodes["context"], "isaacsim.ros2.bridge.ROS2Context"), + (nodes["playback"], "omni.graph.action.OnPlaybackTick"), + (nodes["create_rp"], "isaacsim.core.nodes.IsaacCreateRenderProduct"), + (nodes["rgb"], "isaacsim.ros2.bridge.ROS2CameraHelper"), + (nodes["frame"], "omni.graph.nodes.ConstantString"), + (nodes["topic"], "omni.graph.nodes.ConstantString"), + ], + og.Controller.Keys.CONNECT: [ + (f"{nodes['playback']}.outputs:tick", f"{nodes['create_rp']}.inputs:execIn"), + (f"{nodes['create_rp']}.outputs:execOut", f"{nodes['rgb']}.inputs:execIn"), + (f"{nodes['create_rp']}.outputs:renderProductPath", + f"{nodes['rgb']}.inputs:renderProductPath"), + (f"{nodes['context']}.outputs:context", f"{nodes['rgb']}.inputs:context"), + (f"{nodes['frame']}.inputs:value", f"{nodes['rgb']}.inputs:frameId"), + (f"{nodes['topic']}.inputs:value", f"{nodes['rgb']}.inputs:topicName"), + ], + og.Controller.Keys.SET_VALUES: [ + (("inputs:domain_id", nodes["context"]), int(domain_id)), + (("inputs:cameraPrim", nodes["create_rp"]), camera_prim_path), + (("inputs:width", nodes["create_rp"]), int(width)), + (("inputs:height", nodes["create_rp"]), int(height)), + (("inputs:type", nodes["rgb"]), "rgb"), + (("inputs:value", nodes["frame"]), str(frame_id)), + (("inputs:value", nodes["topic"]), str(topic)), + ], + }, + ) + print(f"[scene_prep] Camera image publisher wired: {camera_prim_path} -> {topic} " + f"({width}x{height} raw Image, domain_id={domain_id})") + + # --------------------------------------------------------------------------- # Consolidate root prims under /World # --------------------------------------------------------------------------- diff --git a/stacks/full_default/launch/stack.launch.xml b/stacks/full_default/launch/stack.launch.xml index b1ab0584b..1f519e8e0 100644 --- a/stacks/full_default/launch/stack.launch.xml +++ b/stacks/full_default/launch/stack.launch.xml @@ -108,9 +108,9 @@ + Swapping the global planner = replacing this include, e.g. with the + asm_exploration_planner module's exploration_planner.launch.xml plus + the global_plan_navigate_bridge (see stacks/full_exploration). --> diff --git a/stacks/full_droan/launch/stack.launch.xml b/stacks/full_droan/launch/stack.launch.xml index 87604bd24..9d814d583 100644 --- a/stacks/full_droan/launch/stack.launch.xml +++ b/stacks/full_droan/launch/stack.launch.xml @@ -108,9 +108,9 @@ + Swapping the global planner = replacing this include, e.g. with the + asm_exploration_planner module's exploration_planner.launch.xml plus + the global_plan_navigate_bridge (see stacks/full_exploration). --> diff --git a/stacks/full_droan_cpu/launch/stack.launch.xml b/stacks/full_droan_cpu/launch/stack.launch.xml index c17181ae9..e71f642c7 100644 --- a/stacks/full_droan_cpu/launch/stack.launch.xml +++ b/stacks/full_droan_cpu/launch/stack.launch.xml @@ -115,9 +115,9 @@ + Swapping the global planner = replacing this include, e.g. with the + asm_exploration_planner module's exploration_planner.launch.xml plus + the global_plan_navigate_bridge (see stacks/full_exploration). --> diff --git a/stacks/full_exploration/README.md b/stacks/full_exploration/README.md new file mode 100644 index 000000000..8101f757a --- /dev/null +++ b/stacks/full_exploration/README.md @@ -0,0 +1,54 @@ +# full_exploration + +`full_default` with the global planner swapped: the random-walk planner is +replaced by the **frontier-based geometric exploration planner** from the +external **asm_exploration_planner** module — the planner the Construction +Project flew on a real construction site. It integrates the filtered Ouster +cloud into its own OpenVDB occupancy map, extracts and clusters frontiers, +samples (optionally bounded) viewpoints around them, and publishes a +shortened, collision-checked RRT path on `global_plan`. That planner only +*publishes* plans; the MIGHTY bridge (`asm_mighty`) follows the `global_plan` +topic directly, so no Path→NavigateTask adapter is needed (a droan_gl-based +variant would add trunk's `global_plan_navigate_bridge`). Everything else — +MIGHTY local planner + acl-mapping world model (LiDAR-based, so it works in +unlit scenes), trajectory controller, PID, safety monitor, takeoff/landing, +VDB mapping, GCS bridge — is unchanged from `full_default`. + +The only differences vs `full_default` are the global-planner include in +`launch/stack.launch.xml` and the `asm_exploration_planner` pin in +`modules.repos`. + +Bring-up: + +```bash +# module pins are reconciled by `airstack up` (or: airstack module sync) +airstack up --stack full_exploration --sim isaac +airstack ready +``` + +Then, after takeoff, enable exploration from the GCS Tasks panel (the +`behavior/global_plan_toggle` service). Exploration runs until toggled off. + +Notes: + + +- Neither module needs a composed image layer: OpenVDB, PCL and Eigen ship + in the trunk robot image, and since 0.21.0-dev.11 so does MIGHTY's + `nlohmann-json3-dev` header dep. `airstack up --stack full_exploration` + reconciles the stack's module pins itself (adds/syncs missing modules). +- Planner tuning lives in the module: `exploration_planner/config/ + exploration_planner.yaml` (open-world defaults) and + `exploration_planner_construction_site.yaml` (bounded exploration inside + site extents, conservative unknown-as-occupied collision checks). Select + the latter by passing `exploration_config` on the module include. +- `path_end_threshold_m` (default 3.0 m) is the radius around the plan's + final pose inside which the planner considers the plan complete and picks + the next frontier; keep it above the local planner's arrival tolerance + (MIGHTY's follower stops ~1–2 m short of the route end). +- `vdb_mapping` still runs: the GCS/RViz layouts and the topic keepalive + consume its map, even though the planner does not. +- `wiring.md` is not committed yet (bootstrap-deferred): generate it from the + first validated run with + `airstack test -m wiring --stack full_exploration --sim isaacsim --num-robots 1` + and commit the observed graph. +- Module repo: [castacks/asm_exploration_planner](https://github.com/castacks/asm_exploration_planner). diff --git a/stacks/full_exploration/docker-compose.yaml b/stacks/full_exploration/docker-compose.yaml new file mode 100644 index 000000000..4588bf25a --- /dev/null +++ b/stacks/full_exploration/docker-compose.yaml @@ -0,0 +1,11 @@ +# Per-stack image composition: the P4 machinery +# (tools/compose_module_layers.py) composes per-module dependency layers on +# top of the trunk base image and emits a compose override for `airstack up`. +# +# full_exploration pins the asm_exploration_planner module (see +# modules.repos). The module declares NO extra apt/pip deps — OpenVDB, PCL and +# Eigen are already in the trunk robot image via its package.xml rosdep keys +# — so there is no dependency layer to compose and nothing is committed here. +# The empty services map keeps this file valid YAML for the stack-anatomy +# contract (tests/meta/test_stack_layout_contract.py). +services: {} diff --git a/stacks/full_exploration/launch/stack.launch.xml b/stacks/full_exploration/launch/stack.launch.xml new file mode 100644 index 000000000..45d0d0ff4 --- /dev/null +++ b/stacks/full_exploration/launch/stack.launch.xml @@ -0,0 +1,154 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stacks/full_exploration/modules.repos b/stacks/full_exploration/modules.repos new file mode 100644 index 000000000..45653de94 --- /dev/null +++ b/stacks/full_exploration/modules.repos @@ -0,0 +1,26 @@ +# modules.repos : module pins for the full_exploration stack. +# +# vcstool format, PINNED to tags/commits (never branches). `airstack module sync` +# reads this file into the gitignored modules/ dir; a stack with a pinned .repos +# IS a localized release set. +# +# airstack_compat is a top-level sibling of repositories: (vcstool ignores it, +# AirStack tooling reads it) declaring the trunk semver range this stack was +# tested against. sync warns on mismatch; it never gates. +# +# full_exploration pulls the asm_exploration_planner module (frontier-based +# geometric exploration global planner); everything else — including the +# global_plan_navigate_bridge that turns its plans into NavigateTask goals — +# is trunk-resident. v0.1.1 is the module's first tag; move the pin +# deliberately (airstack module add --version ). +airstack_compat: ">=0.21.0-dev.10 <0.22.0" +repositories: + asm_exploration_planner: + type: git + url: git@github.com:castacks/asm_exploration_planner.git + version: v0.1.1 + asm_mighty: + type: git + url: https://github.com/castacks/asm_mighty + version: v0.1.5 +x-local-modules: [] diff --git a/stacks/full_macvo/launch/stack.launch.xml b/stacks/full_macvo/launch/stack.launch.xml index 5885cc7fe..66c2cb170 100644 --- a/stacks/full_macvo/launch/stack.launch.xml +++ b/stacks/full_macvo/launch/stack.launch.xml @@ -122,9 +122,9 @@ + Swapping the global planner = replacing this include, e.g. with the + asm_exploration_planner module's exploration_planner.launch.xml plus + the global_plan_navigate_bridge (see stacks/full_exploration). --> diff --git a/stacks/full_raven/README.md b/stacks/full_raven/README.md new file mode 100644 index 000000000..9e781ff00 --- /dev/null +++ b/stacks/full_raven/README.md @@ -0,0 +1,69 @@ +# full_raven + +`full_default` with the global planner swapped: the random-walk planner +(`random_walk_planner`) is replaced by **RAVEN** — *Resilient Aerial +Navigation via Open-Set Semantic Memory and Behavior Adaptation* (Kim, +Alama, Kurdydyk, Keller, Keetha, Wang, Bisk, Scherer — CMU AirLab, ICRA +2026) — from the external **asm_raven** module. RAVEN builds a RayFronts +semantic ray-frontier map from the ZED left RGB + ground-truth depth and +picks a behavior each frame (voxel-based → ray-based → LVLM-guided → +frontier exploration) to publish a semantic `global_plan` toward an +operator's open-vocabulary prompt ("fire hydrant"). The rest of the stack +(MIGHTY local planner from the `asm_mighty` module, trajectory controller, +PID, safety monitor, takeoff/landing, GCS) is unchanged from `full_default`. +MIGHTY plans on the Ouster LiDAR, so navigation keeps working in dark or +untextured scenes where stereo disparity (the DROAN planners' input) does not. + +The module has two halves: + +- a **GPU sidecar container** (`raven`, ROS 2 Humble image, ~10 GB) running + the RayFronts mapper + behavior manager — declared by the module's compose + fragment and started by `airstack up` while the module is synced; +- **`raven_bridge`** in the robot container, relaying the sidecar's plan onto + the canonical `global_plan` and holding the operator prompt. + +RAVEN publishes plans on a topic (it is not a NavigateTask client); the +MIGHTY bridge follows the `global_plan` topic directly, so no adapter is +needed. The only difference vs `full_default` is the global-planner include in +`launch/stack.launch.xml` plus the `asm_raven` pin in `modules.repos` (a +droan_gl-based variant would add trunk's `global_plan_navigate_bridge`). + +Bring-up: + +```bash +airstack module sync # pulls asm_raven (+ its RayFronts submodule) per this stack's modules.repos +# first run only: build the sidecar image (~15 min on a 32-core host, ~10 GB); also happens +# implicitly on the first `airstack up` +docker compose -f docker-compose.yaml -f .airstack/generated/docker-compose.modules.yaml build raven +airstack up --stack full_raven --sim isaac --scene retro-neighborhood +airstack ready +``` + +Then set a semantic target (empty string clears → pure frontier exploration): + +```bash +ros2 topic pub --once /robot_1/raven/set_prompt std_msgs/msg/String "{data: 'fire hydrant'}" +ros2 service call /robot_1/raven/clear_prompt std_srvs/srv/Trigger +``` + +Notes: + + +- RAVEN's scenes are the AirLab Nucleus stages already in + `simulation/scenes.yaml`: `retro-neighborhood`, `construction-site`, + `abandoned-factory` (spawn pose via `ISAAC_SIM_SPAWN_XY`). +- Single robot (`robot_1`, DDS domain 1) by default: the sidecar's + `RAVEN_ROBOT_NAME` / `RAVEN_ROS_DOMAIN_ID` select another; RAVEN's + visualization topics are global (`/filtered_voxel_bbox`, `/mode_text`, ...). +- The first sidecar start downloads the RADIO and SigLIP models (torch hub / + Hugging Face) into `modules/asm_raven/cache/`, which persists across + container recreations. +- The LVLM-guided behavior needs RAVEN's optional external LVLM server + (`/lvlm_output`); without one it degrades gracefully (falls through to + frontier exploration). `RAVEN_BEHAVIORS=voxel,ray,frontier` selects the + no-LVLM ablation explicitly. +- Sidecar rviz layout: `raven_bridge/rviz/raven.rviz` in the module. +- `AIRSTACK_NO_MODULE_COMPOSE=1 airstack up ...` brings the stack up without + the sidecar (raven_bridge then idles). +- Module repo: [castacks/asm_raven](https://github.com/castacks/asm_raven) + (pin `v0.1.0` is a placeholder until the first tag). diff --git a/stacks/full_raven/docker-compose.yaml b/stacks/full_raven/docker-compose.yaml new file mode 100644 index 000000000..14a54c1ff --- /dev/null +++ b/stacks/full_raven/docker-compose.yaml @@ -0,0 +1,13 @@ +# Per-stack image composition: the P4 machinery +# (tools/compose_module_layers.py) composes per-module dependency layers on +# top of the trunk base image and emits a compose override for `airstack up`. +# +# full_raven pins the asm_raven module (see modules.repos). asm_raven adds no +# robot-image layer (raven_bridge is plain rclpy); its heavy half — the +# RayFronts GPU SIDECAR container — is declared by the module's own compose +# fragment (asm_raven/compose/raven.yaml), which `airstack module sync` +# merges into .airstack/generated/docker-compose.modules.yaml and `airstack +# up` includes automatically. Nothing is committed here. The empty services +# map keeps this file valid YAML for the stack-anatomy contract +# (tests/meta/test_stack_layout_contract.py). +services: {} diff --git a/stacks/full_raven/launch/stack.launch.xml b/stacks/full_raven/launch/stack.launch.xml new file mode 100644 index 000000000..91aad1c75 --- /dev/null +++ b/stacks/full_raven/launch/stack.launch.xml @@ -0,0 +1,155 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stacks/full_raven/modules.repos b/stacks/full_raven/modules.repos new file mode 100644 index 000000000..976037129 --- /dev/null +++ b/stacks/full_raven/modules.repos @@ -0,0 +1,27 @@ +# modules.repos : module pins for the full_raven stack. +# +# vcstool format, PINNED to tags/commits (never branches). `airstack module sync` +# reads this file into the gitignored modules/ dir; a stack with a pinned .repos +# IS a localized release set. +# +# airstack_compat is a top-level sibling of repositories: (vcstool ignores it, +# AirStack tooling reads it) declaring the trunk semver range this stack was +# tested against. sync warns on mismatch; it never gates. +# +# full_raven pulls the asm_raven module (RAVEN semantic global planner: the +# RayFronts sidecar container + raven_bridge relay); everything else is +# trunk-resident. asm_raven vendors the RayFronts fork as a git submodule — +# `vcs import --recursive` (what sync runs) pulls it automatically. +# PIN: v0.1.1 is tagged at the module's first push / first green +# module CI — update the version below then. +airstack_compat: ">=0.21.0-dev.10 <0.22.0" +repositories: + asm_raven: + type: git + url: git@github.com:castacks/asm_raven.git + version: v0.1.1 + asm_mighty: + type: git + url: https://github.com/castacks/asm_mighty + version: v0.1.5 +x-local-modules: [] diff --git a/tests/colcon_unit_test_packages.yaml b/tests/colcon_unit_test_packages.yaml index 78f761039..92e9a408c 100644 --- a/tests/colcon_unit_test_packages.yaml +++ b/tests/colcon_unit_test_packages.yaml @@ -10,6 +10,7 @@ robot: packages: - lidar_point_cloud_filter + - global_plan_navigate_bridge # Linter skip lives in lidar_point_cloud_filter setup.cfg + test/conftest.py. # ament pytest does not honor PYTEST_ADDOPTS -m. # launch_testing is skipped via PYTEST_DISABLE_PLUGIN_AUTOLOAD in the test. diff --git a/tests/meta/fixtures/modules_index/modules/exploration_planner.yaml b/tests/meta/fixtures/modules_index/modules/exploration_planner.yaml new file mode 100644 index 000000000..aae55a103 --- /dev/null +++ b/tests/meta/fixtures/modules_index/modules/exploration_planner.yaml @@ -0,0 +1,23 @@ +# Registry entry for the exploration_planner module (schema/module-entry.schema.json). +# airstack_compat is the DECLARED range copied from the module's module.yaml; +# the VERIFIED matrix lives in compat/exploration_planner.yaml (CI-stamped only). +name: exploration_planner +repo: https://github.com/castacks/asm_exploration_planner +description: >- + Frontier-based geometric exploration global planner — OpenVDB occupancy map, + frontier clustering, bounded viewpoint sampling and an RRT path to the chosen + viewpoint, published as global_plan; developed for the CMU AirLab + construction-site project +maintainer: ajong@andrew.cmu.edu # Andrew Jong +license: BSD-3-Clause-Clear +type: ros_package +registered_ref: v0.1.1 +airstack_compat: ">=0.21.0-dev.8 <0.22.0" # DECLARED (from module.yaml) +notes: >- + Extracted from trunk (robot/ros_ws/src/global/planners/exploration, history + preserved) together with the bounded-exploration additions from the + construction-site demo branch. Publishes a path on global_plan and serves + the behavior/global_plan_toggle service; the MIGHTY bridge (full_exploration) + follows the published plan directly, while droan_gl-based stacks add the + trunk global_plan_navigate_bridge. Consumed by trunk reference stack + full_exploration. diff --git a/tests/meta/fixtures/modules_index/modules/raven.yaml b/tests/meta/fixtures/modules_index/modules/raven.yaml new file mode 100644 index 000000000..d7018618b --- /dev/null +++ b/tests/meta/fixtures/modules_index/modules/raven.yaml @@ -0,0 +1,22 @@ +# Registry entry for the raven module (schema/module-entry.schema.json). +# airstack_compat is the DECLARED range copied from the module's module.yaml; +# the VERIFIED matrix lives in compat/raven.yaml (CI-stamped only). +name: raven +repo: https://github.com/castacks/asm_raven +description: >- + RAVEN semantic aerial navigation (ICRA 2026) — the RayFronts open-set + semantic ray-frontier map and behavior manager (voxel, ray, LVLM and + frontier search) in a GPU sidecar container publishing global_plan toward a + natural-language target, plus the raven_bridge plan/prompt relay on the robot +maintainer: ajong@andrew.cmu.edu # Andrew Jong +license: BSD-3-Clause-Clear +type: ros_package +registered_ref: v0.1.1 +airstack_compat: ">=0.21.0-dev.8 <0.22.0" # DECLARED (from module.yaml) +notes: >- + Wrapper (raven_bridge, sidecar Dockerfile, compose fragment, test_stack) is + BSD-3-Clause-Clear; the vendored RayFronts fork (seungchan-kim/RayFronts, + branch raven) keeps its own license — see the module's VENDORED.md. The + sidecar is declared through the manifest's compose fragment (first module to + bring its own container) and is built on first `airstack up` (~20 GB image). + Consumed by trunk reference stack full_raven. diff --git a/tests/meta/fixtures/modules_index/stacks/full_exploration.yaml b/tests/meta/fixtures/modules_index/stacks/full_exploration.yaml new file mode 100644 index 000000000..1d22b3b66 --- /dev/null +++ b/tests/meta/fixtures/modules_index/stacks/full_exploration.yaml @@ -0,0 +1,16 @@ +# Registry entry for the full_exploration trunk reference stack (schema/stack-entry.schema.json). +name: full_exploration +repo: https://github.com/castacks/AirStack +path: stacks/full_exploration +description: >- + Full autonomy (full_default topology: MIGHTY LiDAR-based local planner) with + the frontier-based geometric exploration planner (exploration_planner + module) in place of the random walk; everything else is identical to + full_default +airstack_compat: ">=0.21.0-dev.8 <0.22.0" # from the stack's modules.repos airstack_compat key +wiring: stacks/full_exploration/wiring.md # docs site embeds the CI-generated wiring.md from trunk +notes: >- + Requires the exploration_planner module (registry: + modules/exploration_planner.yaml) and the mighty module, both pinned in the + stack's modules.repos. + wiring.md is pending the stack's first validated wiring-snapshot run. diff --git a/tests/meta/fixtures/modules_index/stacks/full_raven.yaml b/tests/meta/fixtures/modules_index/stacks/full_raven.yaml new file mode 100644 index 000000000..3ead03dcf --- /dev/null +++ b/tests/meta/fixtures/modules_index/stacks/full_raven.yaml @@ -0,0 +1,16 @@ +# Registry entry for the full_raven trunk reference stack (schema/stack-entry.schema.json). +name: full_raven +repo: https://github.com/castacks/AirStack +path: stacks/full_raven +description: >- + Full autonomy (full_default topology: MIGHTY LiDAR-based local planner) with + RAVEN open-set semantic navigation as the global planner (RayFronts GPU + sidecar container + raven_bridge); everything else is identical to + full_default +airstack_compat: ">=0.21.0-dev.8 <0.22.0" # from the stack's modules.repos airstack_compat key +wiring: stacks/full_raven/wiring.md # docs site embeds the CI-generated wiring.md from trunk +notes: >- + Requires the raven module (registry: modules/raven.yaml) and the mighty + module, both pinned in the stack's modules.repos; the first `airstack up` + builds the sidecar image. + wiring.md is pending the stack's first validated wiring-snapshot run. diff --git a/tests/meta/launch_lint_allowlist.txt b/tests/meta/launch_lint_allowlist.txt index 3c6548823..417415097 100644 --- a/tests/meta/launch_lint_allowlist.txt +++ b/tests/meta/launch_lint_allowlist.txt @@ -9,12 +9,10 @@ # the lint until its line is removed; adding a NEW line needs the same # scrutiny as an RFC change. # -# What remains: a standalone utility, a vendored driver tree, a module launch -# awaiting its canonical rewrite, and the interface safety boundary (wrapped -# by design until RFC #380 Part 2). +# What remains: a standalone utility, a vendored driver tree, and the +# interface safety boundary (wrapped by design until RFC #380 Part 2). # # Paths are relative to the repo root, one per line; '#' starts a comment. common/ros_packages/airstack_common/launch/playback.launch.xml robot/docker/zed/ws/src/zed_wrapper/launch/zed_camera.launch.py -robot/ros_ws/src/global/planners/exploration/launch/exploration_launch.xml robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py diff --git a/tests/meta/test_module_overlay_contract.py b/tests/meta/test_module_overlay_contract.py index 70cf0ebcd..bd274d959 100644 --- a/tests/meta/test_module_overlay_contract.py +++ b/tests/meta/test_module_overlay_contract.py @@ -217,3 +217,85 @@ def test_add_twice_keeps_single_entry(sandbox): data = read_repos(sandbox) names = [e["name"] for e in data["x-local-modules"]] assert names == ["hello_module"] + + +# ── compose fragments: relative host paths become absolute ────────────────── + +SIDECAR_REL = os.path.join("tests", "fixtures", "modules", "sidecar_module") + + +def write_sidecar_module(sb): + """A module whose compose fragment declares a whole extra service built + from a Dockerfile inside the module (the asm_raven sidecar shape), with + every kind of relative host path compose resolves against the declaring + file's directory.""" + root = sb / SIDECAR_REL + (root / "docker").mkdir(parents=True) + (root / "compose").mkdir() + (root / "env").mkdir() + (root / "module.yaml").write_text( + "name: sidecar_module\n" + "description: Compose-fragment sidecar fixture for overlay path rewriting.\n" + "maintainer: test@example.com\n" + "license: BSD-3-Clause-Clear\n" + "type: ros_package\n" + 'airstack_compat: ">=0.19.0 <0.21.0"\n' + "targets: [robot]\n" + "compose: compose/sidecar.yaml\n", + encoding="utf-8", + ) + (root / "docker" / "Dockerfile.sidecar").write_text("FROM busybox\n", encoding="utf-8") + (root / "env" / "sidecar.env").write_text("X=1\n", encoding="utf-8") + (root / "compose" / "sidecar.yaml").write_text( + "services:\n" + " sidecar:\n" + " image: sidecar:test\n" + " build:\n" + " context: .\n" + " dockerfile: docker/Dockerfile.sidecar\n" + " env_file:\n" + " - ./env/sidecar.env\n" + " - path: env/optional.env\n" + " required: false\n" + " volumes:\n" + " - ./cache:/root/.cache:rw\n" + " - named_vol:/data\n" + " networks: [airstack_network]\n", + encoding="utf-8", + ) + return root + + +def test_compose_fragment_paths_are_absolutized(sandbox): + root = write_sidecar_module(sandbox) + run_airstack(sandbox, "module", "add", SIDECAR_REL) + compose = yaml.safe_load((sandbox / GENERATED_REL).read_text(encoding="utf-8")) + real = os.path.realpath(root) + svc = compose["services"]["sidecar"] + + # build: context "." -> module dir; relative dockerfile resolved against it + assert svc["build"]["context"] == real + assert svc["build"]["dockerfile"] == os.path.join(real, "docker", "Dockerfile.sidecar") + # env_file: strings and {path, required} mappings + assert svc["env_file"][0] == os.path.join(real, "env", "sidecar.env") + assert svc["env_file"][1] == {"path": os.path.join(real, "env", "optional.env"), + "required": False} + # volumes: relative bind absolutized, named volume untouched + assert f"{real}/cache:/root/.cache:rw" in svc["volumes"] + assert "named_vol:/data" in svc["volumes"] + # verbatim passthrough of everything else + assert svc["image"] == "sidecar:test" + assert svc["networks"] == ["airstack_network"] + # nothing in the generated file is still relative to the fragment + text = (sandbox / GENERATED_REL).read_text(encoding="utf-8") + assert "context: ." not in text and "./env" not in text and "./cache" not in text + + +def test_compose_fragment_sync_is_idempotent_and_doctor_clean(sandbox): + write_sidecar_module(sandbox) + run_airstack(sandbox, "module", "add", SIDECAR_REL) + before = (sandbox / GENERATED_REL).read_text(encoding="utf-8") + run_airstack(sandbox, "module", "sync") + assert (sandbox / GENERATED_REL).read_text(encoding="utf-8") == before + code, out = run_airstack(sandbox, "module", "doctor") + assert code == 0 and "overlay OK" in out diff --git a/tools/module_overlay.py b/tools/module_overlay.py index 243147741..beb9d04c0 100755 --- a/tools/module_overlay.py +++ b/tools/module_overlay.py @@ -32,12 +32,19 @@ ``optitrack.natnet.emulator``). - a manifest ``compose:`` fragment → merged verbatim into the generated file, - with relative host paths rewritten to absolute (compose files resolve - relative bind sources against ambiguous bases when merged via ``-f``; the - generated file is machine-local and regenerated on every sync, so absolute - paths are the unambiguous choice). When a fragment is declared it is the - source of truth for that module's mounts — the ``exts/`` auto-mount is - skipped to avoid double mounts (the dfm2 pilot's friction log, item 5). + with relative host paths rewritten to absolute: bind-mount sources, + ``build.context`` (and a relative ``build.dockerfile``, resolved against + the absolute context exactly as compose would), and ``env_file`` entries. + Compose resolves all of these against the directory of the file that + declares them, which for the generated file is ``.airstack/generated/``, + not the module — and relative bind sources resolve against ambiguous bases + when files are merged via ``-f``. The generated file is machine-local and + regenerated on every sync, so absolute paths are the unambiguous choice. + Fragments may declare whole extra services this way — e.g. a GPU sidecar + container built from a Dockerfile inside the module (the asm_raven + RayFronts sidecar). When a fragment is declared it is the source of truth + for that module's mounts — the ``exts/`` auto-mount is skipped to avoid + double mounts (the dfm2 pilot's friction log, item 5). - ``data`` / other types → no overlay action yet (noted). @@ -169,17 +176,73 @@ def desired_isaac_links(root, modules): return links -def _abs_host_path(src, base_dir): - """Rewrite a compose bind source relative to ``base_dir`` into an absolute path.""" - if src.startswith(("./", "../")): +def _abs_host_path(src, base_dir, bare_is_relative=False): + """Rewrite a compose host path relative to ``base_dir`` into an absolute path. + + Bind-mount sources are only rewritten when they are explicitly relative + (``./x``, ``../x``, ``.``) — a bare name there is a named volume. For + ``build.context`` / ``build.dockerfile`` / ``env_file`` (``bare_is_relative``) + any non-absolute, non-URL path is relative to ``base_dir``, as compose + itself would resolve it against the declaring file's directory. + """ + if not isinstance(src, str) or not src: + return src + if src == "." or src.startswith(("./", "../")): + return os.path.normpath(str(base_dir / src)) + if bare_is_relative and not os.path.isabs(src) and "://" not in src \ + and not src.startswith("git@") and not src.startswith("~"): return os.path.normpath(str(base_dir / src)) return src +def _abs_build(build, module_dir): + """Absolutize a compose ``build`` entry (string context or mapping).""" + if isinstance(build, str): + return _abs_host_path(build, module_dir, bare_is_relative=True) + if not isinstance(build, dict): + return build + build = dict(build) + context = _abs_host_path(build.get("context", "."), module_dir, bare_is_relative=True) + build["context"] = context + dockerfile = build.get("dockerfile") + if isinstance(dockerfile, str) and dockerfile and not os.path.isabs(dockerfile) \ + and os.path.isabs(context): + # compose resolves a relative dockerfile against the context dir + build["dockerfile"] = os.path.normpath(os.path.join(context, dockerfile)) + return build + + +def _abs_env_file(env_file, module_dir): + """Absolutize a compose ``env_file`` entry (string, list of strings, or + list of ``{path, required}`` mappings).""" + def one(entry): + if isinstance(entry, str): + return _abs_host_path(entry, module_dir, bare_is_relative=True) + if isinstance(entry, dict) and isinstance(entry.get("path"), str): + return {**entry, "path": _abs_host_path(entry["path"], module_dir, bare_is_relative=True)} + return entry + if isinstance(env_file, list): + return [one(e) for e in env_file] + return one(env_file) + + def _merge_service(target, source, module_dir): """Merge one compose service definition into ``target`` (fragment semantics).""" for key, value in source.items(): - if key == "volumes" and isinstance(value, list): + if key == "build": + target["build"] = _abs_build(value, module_dir) + elif key == "env_file": + value = _abs_env_file(value, module_dir) + if isinstance(value, list): + merged = target.setdefault("env_file", []) + if not isinstance(merged, list): + merged = target["env_file"] = [merged] + for entry in value: + if entry not in merged: + merged.append(entry) + else: + target["env_file"] = value + elif key == "volumes" and isinstance(value, list): merged = target.setdefault("volumes", []) for entry in value: if isinstance(entry, str):