add geofence manager

[darshmenon]

Adds a polygon-based geofence manager node for Nav2.

What it does:

• Subscribes to /amcl_pose and checks if the robot is inside any defined zone
• Publishes /geofence/breach (Bool) when inside a forbidden zone
• Publishes /geofence/current_zone (String) with the active zone name
• Zones defined in config/geofences.yaml — no external deps, pure ray-casting

New files:

• scripts/geofence_manager.py
• config/geofences.yaml

[darshmenon]

Zone config is in config/geofences.yaml — edit polygons there to match your map. Launch with:

ros2 run diff_drive_robot geofence_manager.py --ros-args -p geofence_file:=/path/to/geofences.yaml

Monitor breach alerts:

ros2 topic echo /geofence/breach
ros2 topic echo /geofence/current_zone

[darshmenon]

Geofence Manager for Nav2

Problem

Nav2 has no built-in mechanism to define virtual boundaries or forbidden zones on the map. Without geofencing, robots can freely enter restricted areas — charging bays, hazard zones, or off-limits corridors — with no alerting or enforcement.

Solution

Adds a lightweight geofence_manager node that monitors /amcl_pose and checks the robot's position against user-defined polygon zones loaded from a YAML config. Uses a pure ray-casting algorithm (no external deps beyond rclpy/PyYAML) so it runs on any Humble/Jazzy install.

# before — no boundary enforcement, robot goes anywhere
# Nav2 sends robot to goal, no zone awareness

# after — breach published instantly when robot enters forbidden zone
ros2 topic echo /geofence/breach        # Bool
ros2 topic echo /geofence/current_zone  # String

What changed

• scripts/geofence_manager.py — ROS 2 node, subscribes to /amcl_pose, publishes /geofence/breach and /geofence/current_zone
• config/geofences.yaml — example zone definitions (warehouse floor, charging bay, hazard zone)

Test Plan

• Launch with a real AMCL-localized robot, verify /geofence/current_zone updates as robot moves
• Drive robot into a allowed: false zone, confirm /geofence/breach publishes True
• Verify no crash when geofence_file param is empty or path is invalid
• Test on both Humble and Jazzy

[darshmenon]

Usage & Integration

Launch the node

ros2 run diff_drive_robot geofence_manager.py --ros-args   -p geofence_file:=$(ros2 pkg prefix diff_drive_robot)/share/diff_drive_robot/config/geofences.yaml

Monitor in real time

ros2 topic echo /geofence/breach        # True when in a forbidden zone
ros2 topic echo /geofence/current_zone  # active zone name

Integration with Nav2

Hook /geofence/breach into a behavior tree or a simple subscriber to cancel the current goal when a breach is detected:

self.breach_sub = self.create_subscription(Bool, '/geofence/breach', self._on_breach, 10)

def _on_breach(self, msg):
    if msg.data:
        self.nav.cancelTask()

---

How it fits the stack

SLAM Toolbox → map
      ↓
   AMCL → /amcl_pose ──→ GeofenceManager → /geofence/breach
      ↓                                           ↓
    Nav2 ←──────────── cancel goal on breach ─────┘

Works alongside the existing collision_monitor.py and priority_collision_avoidance.py — geofencing handles zone-level policy, collision monitor handles obstacle-level safety.

---

Planned improvements

• Velocity scaling inside warning zones (slow down before forbidden zone, not just alert)
• RViz2 marker publisher to visualize zone polygons on the map
• Dynamic zone enable/disable via a service call
• Multi-robot support — per-namespace pose topic (robot1/amcl_pose)