Metadata-Version: 2.4
Name: bteng-nav2
Version: 0.1.0
Summary: Nav2 behavior tree nodes for the BTEng engine
Author-email: BTEng contributors <mdirzpr@gmail.com>
License-Expression: Apache-2.0
Project-URL: Homepage, https://github.com/BTEng-dev/bteng_nav2
Project-URL: Documentation, https://github.com/BTEng-dev/bteng_nav2/tree/main/docs
Project-URL: Bug Tracker, https://github.com/BTEng-dev/bteng_nav2/issues
Keywords: behavior-tree,robotics,ros2,nav2,navigation,automation
Classifier: Development Status :: 3 - Alpha
Classifier: Intended Audience :: Developers
Classifier: Operating System :: OS Independent
Classifier: Programming Language :: Python :: 3
Classifier: Programming Language :: Python :: 3.10
Classifier: Programming Language :: Python :: 3.11
Classifier: Programming Language :: Python :: 3.12
Classifier: Topic :: Scientific/Engineering
Classifier: Topic :: Software Development :: Libraries :: Python Modules
Requires-Python: >=3.10
Description-Content-Type: text/markdown
License-File: LICENSE
Requires-Dist: bteng>=0.2.0
Requires-Dist: bteng-ros2>=0.1.0
Provides-Extra: dev
Requires-Dist: build; extra == "dev"
Requires-Dist: pytest; extra == "dev"
Requires-Dist: twine; extra == "dev"
Dynamic: license-file

# bteng-nav2

Nav2 behavior tree nodes for the [BTEng](https://pypi.org/project/bteng/) engine.

A pure Python pip library — no ROS 2 workspace, no colcon, no XML behavior trees required.
Every Nav2 action server, service, and common condition is available as a composable BTEng node.

## Install

```bash
source /opt/ros/humble/setup.bash   # or iron / jazzy
pip install bteng-nav2
```

`bteng` and `bteng-ros2` are pulled in automatically.

---

## What's included

### Actions → `RosActionNode`

| Node | Nav2 server | Ports IN | Ports OUT |
|---|---|---|---|
| `NavigateToPoseNode` | `/navigate_to_pose` | `goal_pose`, `behavior_tree=""` | `result` |
| `NavigateThroughPosesNode` | `/navigate_through_poses` | `poses`, `behavior_tree=""` | `result` |
| `ComputePathToPoseNode` | `/compute_path_to_pose` | `goal`, `start=None`, `planner_id=""` | `path` |
| `ComputePathThroughPosesNode` | `/compute_path_through_poses` | `goals`, `start=None`, `planner_id=""` | `path` |
| `FollowPathNode` | `/follow_path` | `path`, `controller_id=""`, `goal_checker_id=""` | — |
| `SmoothPathNode` | `/smooth_path` | `path`, `smoother_id=""`, `max_smoothing_duration=10.0` | `smoothed_path` |
| `SpinNode` | `/spin` | `target_yaw`, `time_allowance=10.0` | — |
| `BackUpNode` | `/back_up` | `backup_dist=0.15`, `backup_speed=0.025`, `time_allowance=10.0` | — |
| `DriveOnHeadingNode` | `/drive_on_heading` | `dist_to_travel`, `speed`, `time_allowance=10.0` | — |
| `WaitNode` | `/wait` | `wait_duration=1` | — |
| `AssistedTeleopNode` | `/assisted_teleop` | `time_allowance=30.0` | — |
| `DockRobotNode` | `/dock_robot` | `dock_id`, `dock_type=""` | `success` |
| `UndockRobotNode` | `/undock_robot` | `dock_type=""` | — |

### Services → `RosServiceNode`

| Node | Nav2 server | Ports IN | Ports OUT |
|---|---|---|---|
| `ClearGlobalCostmapNode` | `/clear_costmap_global_srv` | — | — |
| `ClearLocalCostmapNode` | `/clear_costmap_local_srv` | — | — |
| `ClearGlobalCostmapAroundRobotNode` | `/clear_global_costmap_around_robot` | `reset_distance=1.0` | — |
| `ClearLocalCostmapAroundRobotNode` | `/clear_local_costmap_around_robot` | `reset_distance=1.0` | — |
| `GetGlobalCostmapNode` | `/get_costmap_global` | — | `costmap` |
| `GetLocalCostmapNode` | `/get_costmap_local` | — | `costmap` |
| `IsPathValidNode` | `/is_path_valid` | `path` | — (SUCCESS/FAILURE) |
| `LoadMapNode` | `/map_server/load_map` | `map_url` | `result_code` |

### Conditions → `RosConditionNode`

| Node | Source | Logic |
|---|---|---|
| `IsRobotNearGoalCondition` | `/amcl_pose` | Euclidean distance to `goal_pose` < `tolerance=0.5` |
| `IsLocalizationActiveCondition` | `/amcl_pose` | Message received within `timeout=2.0` s |
| `IsObstacleNearCondition` | `/scan` | `min(ranges)` < `min_distance=0.5` |
| `IsStuckCondition` | `/odom` | Velocity ≈ 0 for > `stuck_timeout=3.0` s |
| `IsGoalUpdatedCondition` | Blackboard | `goal_pose` key changed since last tick |
| `IsNavigationActiveCondition` | Action server | `/navigate_to_pose` server is available |

### Publishers → `RosTopicMixin`

| Node | Topic | Message |
|---|---|---|
| `PublishGoalPoseNode` | `/goal_pose` | `geometry_msgs/PoseStamped` |
| `SetInitialPoseNode` | `/initialpose` | `geometry_msgs/PoseWithCovarianceStamped` |

---

## Usage

### Navigate to a pose

```python
import rclpy
from bteng import Tree, Blackboard, NodeConfig
from bteng_ros2 import RosBTExecutor
from bteng_nav2 import NavigateToPoseNode
from geometry_msgs.msg import PoseStamped

rclpy.init()
ros_node = rclpy.create_node("bt_nav")

bb = Blackboard()
bb.set("goal_pose", make_pose(x=2.0, y=1.5))

config = NodeConfig(blackboard=bb, input_ports={"goal_pose": "goal_pose"})
root = NavigateToPoseNode("navigate", config=config)

tree = Tree(root=root)
executor = RosBTExecutor(tree, ros_node=ros_node)
executor.run()

rclpy.shutdown()
```

### Compute path then follow it

Wire the output of one node to the input of another through the blackboard:

```python
from bteng import SequenceNode, NodeConfig, Blackboard
from bteng_nav2 import ComputePathToPoseNode, FollowPathNode

bb = Blackboard()
bb.set("goal", goal_pose)

compute_cfg = NodeConfig(
    blackboard=bb,
    input_ports={"goal": "goal"},
    output_ports={"path": "path"},
)
follow_cfg = NodeConfig(
    blackboard=bb,
    input_ports={"path": "path"},
)

root = SequenceNode("root", children=[
    ComputePathToPoseNode("compute", config=compute_cfg),
    FollowPathNode("follow", config=follow_cfg),
])
```

### Recovery tree

```python
from bteng import FallbackNode, SequenceNode
from bteng_nav2 import (
    NavigateToPoseNode, ClearGlobalCostmapNode, BackUpNode, SpinNode
)

root = FallbackNode("root", children=[
    NavigateToPoseNode("navigate", config=nav_cfg),
    SequenceNode("recovery", children=[
        ClearGlobalCostmapNode("clear", config=cfg),
        BackUpNode("back_up", config=cfg),
        SpinNode("spin", config=cfg),
        NavigateToPoseNode("retry", config=nav_cfg),
    ]),
])
```

### Full autonomy with conditions

```python
from bteng import FallbackNode, SequenceNode
from bteng_nav2 import (
    IsLocalizationActiveCondition,
    IsRobotNearGoalCondition,
    IsNavigationActiveCondition,
    NavigateToPoseNode,
    ClearGlobalCostmapNode,
    SpinNode,
)

root = FallbackNode("root", children=[
    SequenceNode("pre_checks", children=[
        IsLocalizationActiveCondition("localized", config=cfg),
        IsRobotNearGoalCondition("near_goal", config=cfg),  # already there
    ]),
    SequenceNode("navigate", children=[
        IsNavigationActiveCondition("nav_ready", config=cfg),
        NavigateToPoseNode("go", config=cfg),
    ]),
    SequenceNode("recover", children=[
        ClearGlobalCostmapNode("clear", config=cfg),
        SpinNode("spin", config=cfg),
    ]),
])
```

---

## Design

`bteng-nav2` follows the same mixin pattern as `bteng-ros2`:

- **Action nodes** subclass `RosActionNode` and implement `make_goal()`. All nav2 action lifecycle (send, poll, cancel) is handled.
- **Service nodes** subclass `RosServiceNode` and implement `make_request()`. Response handling goes in `on_response()`.
- **Condition nodes** subclass `RosConditionNode` and implement `evaluate(msg)`. Subscriptions are created lazily on the first tick.
- **Publisher nodes** mix `RosTopicMixin` with `ActionNode`. Publishers are created lazily and reused.

All ROS message imports are **lazy** (inside methods), so the package imports cleanly without a ROS installation — enabling unit testing via standard `pytest`.

---

## Testing without ROS 2

The test suite runs fully offline:

```bash
python -m venv .venv && source .venv/bin/activate
pip install -e ".[dev]"
pytest
```

`test/conftest.py` stubs all ROS 2 and Nav2 message types so no ROS installation is needed.

---

## Examples

| File | Demonstrates |
|---|---|
| `examples/01_navigate_to_pose.py` | Single goal navigation |
| `examples/02_compute_and_follow.py` | Path planning + path following pipeline |
| `examples/03_patrol_loop.py` | Multi-waypoint patrol loop |
| `examples/04_recovery_tree.py` | Navigation with costmap clear + backup fallback |
| `examples/05_full_autonomy.py` | Localization check, goal check, navigation, recovery |

Run any example after sourcing ROS 2:

```bash
source /opt/ros/humble/setup.bash
python examples/01_navigate_to_pose.py
```

---

## Requirements

- Python ≥ 3.10
- ROS 2 Humble / Iron / Jazzy
- `bteng >= 0.2.0`
- `bteng-ros2 >= 0.1.0`
- Nav2 (`nav2_msgs`, `nav_msgs`, `sensor_msgs`, `geometry_msgs`)

---

## License

Apache 2.0
