sl-controls
bfd291cbdd
Implements ROS2 geofence enforcer for SaltyBot with: - Loads polygon geofence from params (list of x/y vertices) - Subscribes to /odom for real-time robot position - Point-in-polygon ray casting algorithm for boundary checking - Publishes Bool on /saltybot/geofence_breach on boundary violation - Optional enforcement flag for cmd_vel zeroing - Configurable safety margin - Includes 20+ unit tests for geometry and breach detection Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
171 lines
5.4 KiB
Python
171 lines
5.4 KiB
Python
"""Tests for geofence boundary enforcer."""
|
|
|
|
import pytest
|
|
from nav_msgs.msg import Odometry
|
|
from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance, TwistWithCovariance
|
|
import rclpy
|
|
from rclpy.time import Time
|
|
|
|
from saltybot_geofence.geofence_node import GeofenceNode
|
|
|
|
|
|
@pytest.fixture
|
|
def rclpy_fixture():
|
|
rclpy.init()
|
|
yield
|
|
rclpy.shutdown()
|
|
|
|
|
|
@pytest.fixture
|
|
def node(rclpy_fixture):
|
|
node = GeofenceNode()
|
|
yield node
|
|
node.destroy_node()
|
|
|
|
|
|
class TestInit:
|
|
def test_node_initialization(self, node):
|
|
assert node.enforce_boundary is False
|
|
assert node.margin == 0.0
|
|
assert node.inside_geofence is True
|
|
|
|
|
|
class TestVertexParsing:
|
|
def test_parse_vertices_valid(self, node):
|
|
vertices = node._parse_vertices([0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0])
|
|
assert len(vertices) == 4
|
|
assert vertices[0] == (0.0, 0.0)
|
|
assert vertices[1] == (1.0, 0.0)
|
|
|
|
def test_parse_vertices_insufficient(self, node):
|
|
vertices = node._parse_vertices([0.0, 0.0, 1.0])
|
|
assert len(vertices) == 0
|
|
|
|
|
|
class TestPointInPolygon:
|
|
def test_point_inside_square(self, node):
|
|
vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
|
|
assert node._point_in_polygon((1.0, 1.0), vertices) is True
|
|
|
|
def test_point_outside_square(self, node):
|
|
vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
|
|
assert node._point_in_polygon((3.0, 1.0), vertices) is False
|
|
|
|
def test_point_on_vertex(self, node):
|
|
vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
|
|
# Point on vertex behavior may vary (typically outside)
|
|
result = node._point_in_polygon((0.0, 0.0), vertices)
|
|
assert isinstance(result, bool)
|
|
|
|
def test_point_on_edge(self, node):
|
|
vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
|
|
# Point on edge behavior (typically outside)
|
|
result = node._point_in_polygon((1.0, 0.0), vertices)
|
|
assert isinstance(result, bool)
|
|
|
|
def test_triangle_inside(self, node):
|
|
vertices = [(0.0, 0.0), (4.0, 0.0), (2.0, 3.0)]
|
|
assert node._point_in_polygon((2.0, 1.0), vertices) is True
|
|
|
|
def test_triangle_outside(self, node):
|
|
vertices = [(0.0, 0.0), (4.0, 0.0), (2.0, 3.0)]
|
|
assert node._point_in_polygon((5.0, 5.0), vertices) is False
|
|
|
|
def test_concave_polygon(self, node):
|
|
# L-shaped polygon
|
|
vertices = [(0.0, 0.0), (3.0, 0.0), (3.0, 1.0), (1.0, 1.0), (1.0, 3.0), (0.0, 3.0)]
|
|
assert node._point_in_polygon((0.5, 0.5), vertices) is True
|
|
assert node._point_in_polygon((2.0, 2.0), vertices) is False
|
|
|
|
def test_circle_approximation(self, node):
|
|
# Octagon approximating circle
|
|
import math
|
|
vertices = []
|
|
for i in range(8):
|
|
angle = 2 * math.pi * i / 8
|
|
vertices.append((math.cos(angle), math.sin(angle)))
|
|
|
|
# Center should be inside
|
|
assert node._point_in_polygon((0.0, 0.0), vertices) is True
|
|
# Far outside should be outside
|
|
assert node._point_in_polygon((10.0, 10.0), vertices) is False
|
|
|
|
|
|
class TestOdometryProcessing:
|
|
def test_odometry_update_position(self, node):
|
|
node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
|
|
|
|
msg = Odometry()
|
|
msg.pose.pose.position.x = 5.0
|
|
msg.pose.pose.position.y = 5.0
|
|
|
|
node._on_odometry(msg)
|
|
|
|
assert node.robot_x == 5.0
|
|
assert node.robot_y == 5.0
|
|
|
|
def test_breach_detection_inside(self, node):
|
|
node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
|
|
|
|
msg = Odometry()
|
|
msg.pose.pose.position.x = 5.0
|
|
msg.pose.pose.position.y = 5.0
|
|
|
|
node._on_odometry(msg)
|
|
|
|
assert node.inside_geofence is True
|
|
|
|
def test_breach_detection_outside(self, node):
|
|
node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
|
|
|
|
msg = Odometry()
|
|
msg.pose.pose.position.x = 15.0
|
|
msg.pose.pose.position.y = 5.0
|
|
|
|
node._on_odometry(msg)
|
|
|
|
assert node.inside_geofence is False
|
|
|
|
def test_breach_flag_transition(self, node):
|
|
node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
|
|
assert node.breach_published is False
|
|
|
|
# Move outside
|
|
msg = Odometry()
|
|
msg.pose.pose.position.x = 15.0
|
|
msg.pose.pose.position.y = 5.0
|
|
node._on_odometry(msg)
|
|
|
|
assert node.breach_published is True
|
|
|
|
# Move back inside
|
|
msg.pose.pose.position.x = 5.0
|
|
msg.pose.pose.position.y = 5.0
|
|
node._on_odometry(msg)
|
|
|
|
assert node.breach_published is False
|
|
|
|
|
|
class TestEdgeCases:
|
|
def test_no_geofence_defined(self, node):
|
|
node.geofence_vertices = []
|
|
|
|
msg = Odometry()
|
|
msg.pose.pose.position.x = 0.0
|
|
msg.pose.pose.position.y = 0.0
|
|
|
|
node._on_odometry(msg)
|
|
|
|
# Should default to safe (inside)
|
|
assert node.inside_geofence is True
|
|
|
|
def test_very_small_polygon(self, node):
|
|
vertices = [(0.0, 0.0), (0.01, 0.0), (0.01, 0.01), (0.0, 0.01)]
|
|
assert node._point_in_polygon((0.005, 0.005), vertices) is True
|
|
assert node._point_in_polygon((0.1, 0.1), vertices) is False
|
|
|
|
def test_large_coordinates(self, node):
|
|
vertices = [(0.0, 0.0), (1000.0, 0.0), (1000.0, 1000.0), (0.0, 1000.0)]
|
|
assert node._point_in_polygon((500.0, 500.0), vertices) is True
|
|
assert node._point_in_polygon((1500.0, 500.0), vertices) is False
|