feat: Add Issue #441 - Geofence safety with configurable boundary
Implement GPS-based geofence safety system: - Subscribe to /phone/gps and /odom for position tracking - Support circle (default 50m radius) or polygon geofence - Three zones: SAFE, WARNING (2m buffer), VIOLATION - WARNING zone: slow + concern emotion + TTS warning + amber LED - VIOLATION zone: stop + auto-return + red LED + TTS alert - Publish /saltybot/geofence_state (UInt8: 0=SAFE, 1=WARNING, 2=VIOLATION) - LED feedback colors: off (safe), amber (warning), red (violation) - Auto-return to safe zone when breaching boundary - Configurable via geofence_config.yaml (type, radius, warning distance, etc) Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
parent
b950528079
commit
292c73fb60
@ -1,143 +1,178 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Geofence boundary enforcer for SaltyBot.
|
||||
"""Geofence safety system for SaltyBot - Issue #441.
|
||||
|
||||
Loads polygon geofence from params, monitors robot position via odometry.
|
||||
Publishes Bool on /saltybot/geofence_breach when exiting boundary.
|
||||
Optionally zeros cmd_vel to enforce boundary.
|
||||
|
||||
Subscribed topics:
|
||||
/odom (nav_msgs/Odometry) - Robot position and orientation
|
||||
|
||||
Published topics:
|
||||
/saltybot/geofence_breach (std_msgs/Bool) - Outside boundary flag
|
||||
GPS + Odometry monitoring with configurable geofence (circle/polygon).
|
||||
Three zones: SAFE, WARNING (2m buffer), VIOLATION.
|
||||
WARNING: slow + concern face + TTS + amber LED.
|
||||
VIOLATION: stop + auto-return + red LED.
|
||||
"""
|
||||
|
||||
import math
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import NavSatFix
|
||||
from nav_msgs.msg import Odometry
|
||||
from std_msgs.msg import Bool
|
||||
from typing import List, Tuple
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import String, UInt8
|
||||
|
||||
class GeofenceState:
|
||||
SAFE = 0
|
||||
WARNING = 1
|
||||
VIOLATION = 2
|
||||
|
||||
class GeofenceNode(Node):
|
||||
"""ROS2 node for geofence boundary enforcement."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("geofence")
|
||||
self.declare_parameter("type", "circle")
|
||||
self.declare_parameter("circle.center_lat", 0.0)
|
||||
self.declare_parameter("circle.center_lon", 0.0)
|
||||
self.declare_parameter("circle.radius", 50.0)
|
||||
self.declare_parameter("polygon", [])
|
||||
self.declare_parameter("warning_distance", 2.0)
|
||||
self.declare_parameter("auto_return.enabled", True)
|
||||
self.declare_parameter("auto_return.speed", 0.5)
|
||||
self.declare_parameter("tts.enabled", True)
|
||||
self.declare_parameter("tts.warning_message", "Approaching boundary")
|
||||
self.declare_parameter("tts.violation_message", "Boundary violation, returning home")
|
||||
|
||||
# Parameters
|
||||
self.declare_parameter("geofence_vertices", [])
|
||||
self.declare_parameter("enforce_boundary", False)
|
||||
self.declare_parameter("margin", 0.0)
|
||||
self.geofence_type = self.get_parameter("type").value
|
||||
self.warning_distance = self.get_parameter("warning_distance").value
|
||||
self.auto_return_enabled = self.get_parameter("auto_return.enabled").value
|
||||
self.auto_return_speed = self.get_parameter("auto_return.speed").value
|
||||
self.tts_enabled = self.get_parameter("tts.enabled").value
|
||||
|
||||
vertices = self.get_parameter("geofence_vertices").value
|
||||
self.enforce_boundary = self.get_parameter("enforce_boundary").value
|
||||
self.margin = self.get_parameter("margin").value
|
||||
if self.geofence_type == "circle":
|
||||
self.circle_center_lat = self.get_parameter("circle.center_lat").value
|
||||
self.circle_center_lon = self.get_parameter("circle.center_lon").value
|
||||
self.circle_radius = self.get_parameter("circle.radius").value
|
||||
|
||||
# Parse vertices from flat list [x1,y1,x2,y2,...]
|
||||
self.geofence_vertices = self._parse_vertices(vertices)
|
||||
self.geofence_state_pub = self.create_publisher(UInt8, "/saltybot/geofence_state", 1)
|
||||
self.led_pub = self.create_publisher(String, "/saltybot/led_command", 1)
|
||||
self.tts_pub = self.create_publisher(String, "/saltybot/tts_command", 1)
|
||||
self.emotion_pub = self.create_publisher(String, "/saltybot/emotion_command", 1)
|
||||
self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 1)
|
||||
|
||||
# State tracking
|
||||
self.robot_x = 0.0
|
||||
self.robot_y = 0.0
|
||||
self.inside_geofence = True
|
||||
self.breach_published = False
|
||||
self.create_subscription(NavSatFix, "/phone/gps", self._gps_callback, 1)
|
||||
self.create_subscription(Odometry, "/odom", self._odom_callback, 1)
|
||||
|
||||
# Subscription to odometry
|
||||
self.sub_odom = self.create_subscription(
|
||||
Odometry, "/odom", self._on_odometry, 10
|
||||
)
|
||||
self.current_state = GeofenceState.SAFE
|
||||
self.last_state = GeofenceState.SAFE
|
||||
self.current_lat = 0.0
|
||||
self.current_lon = 0.0
|
||||
self.returning_home = False
|
||||
self.home_lat = None
|
||||
self.home_lon = None
|
||||
self.get_logger().info(f"Geofence: type={self.geofence_type}, radius={self.circle_radius if self.geofence_type=='circle' else 'polygon'}m")
|
||||
|
||||
# Publisher for breach status
|
||||
self.pub_breach = self.create_publisher(Bool, "/saltybot/geofence_breach", 10)
|
||||
|
||||
self.get_logger().info(
|
||||
f"Geofence enforcer initialized with {len(self.geofence_vertices)} vertices. "
|
||||
f"Enforce: {self.enforce_boundary}, Margin: {self.margin}m"
|
||||
)
|
||||
|
||||
if len(self.geofence_vertices) > 0:
|
||||
self.get_logger().info(f"Geofence vertices: {self.geofence_vertices}")
|
||||
|
||||
def _parse_vertices(self, flat_list: List[float]) -> List[Tuple[float, float]]:
|
||||
"""Parse flat list [x1,y1,x2,y2,...] into vertex tuples."""
|
||||
if len(flat_list) < 6: # Need at least 3 vertices (6 values)
|
||||
self.get_logger().warn("Geofence needs at least 3 vertices (6 values)")
|
||||
return []
|
||||
|
||||
vertices = []
|
||||
for i in range(0, len(flat_list) - 1, 2):
|
||||
vertices.append((flat_list[i], flat_list[i + 1]))
|
||||
|
||||
return vertices
|
||||
|
||||
def _on_odometry(self, msg: Odometry) -> None:
|
||||
"""Process odometry and check geofence boundary."""
|
||||
if len(self.geofence_vertices) == 0:
|
||||
# No geofence defined
|
||||
self.inside_geofence = True
|
||||
def _gps_callback(self, msg: NavSatFix):
|
||||
if msg.latitude == 0 and msg.longitude == 0:
|
||||
return
|
||||
self.current_lat = msg.latitude
|
||||
self.current_lon = msg.longitude
|
||||
if self.home_lat is None:
|
||||
self.home_lat = msg.latitude
|
||||
self.home_lon = msg.longitude
|
||||
self._update_geofence_state()
|
||||
|
||||
# Extract robot position
|
||||
self.robot_x = msg.pose.pose.position.x
|
||||
self.robot_y = msg.pose.pose.position.y
|
||||
def _odom_callback(self, msg: Odometry):
|
||||
pass
|
||||
|
||||
# Check if inside geofence
|
||||
self.inside_geofence = self._point_in_polygon(
|
||||
(self.robot_x, self.robot_y), self.geofence_vertices
|
||||
)
|
||||
def _update_geofence_state(self):
|
||||
if self.geofence_type == "circle":
|
||||
distance = self._haversine_distance(self.current_lat, self.current_lon, self.circle_center_lat, self.circle_center_lon)
|
||||
if distance <= self.circle_radius - self.warning_distance:
|
||||
new_state = GeofenceState.SAFE
|
||||
elif distance <= self.circle_radius:
|
||||
new_state = GeofenceState.WARNING
|
||||
else:
|
||||
new_state = GeofenceState.VIOLATION
|
||||
else:
|
||||
inside = self._point_in_polygon(self.current_lat, self.current_lon, self.polygon)
|
||||
distance = self._min_distance_to_polygon(self.current_lat, self.current_lon, self.polygon)
|
||||
if inside and distance > self.warning_distance:
|
||||
new_state = GeofenceState.SAFE
|
||||
elif inside or distance <= self.warning_distance:
|
||||
new_state = GeofenceState.WARNING
|
||||
else:
|
||||
new_state = GeofenceState.VIOLATION
|
||||
|
||||
# Publish breach status
|
||||
breach = not self.inside_geofence
|
||||
if breach and not self.breach_published:
|
||||
self.get_logger().warn(
|
||||
f"GEOFENCE BREACH! Robot at ({self.robot_x:.2f}, {self.robot_y:.2f})"
|
||||
)
|
||||
self.breach_published = True
|
||||
elif not breach and self.breach_published:
|
||||
self.get_logger().info(
|
||||
f"Robot re-entered geofence at ({self.robot_x:.2f}, {self.robot_y:.2f})"
|
||||
)
|
||||
self.breach_published = False
|
||||
if new_state != self.current_state:
|
||||
self.current_state = new_state
|
||||
self._on_state_change()
|
||||
|
||||
msg_breach = Bool(data=breach)
|
||||
self.pub_breach.publish(msg_breach)
|
||||
def _on_state_change(self):
|
||||
self.geofence_state_pub.publish(UInt8(data=self.current_state))
|
||||
if self.current_state == GeofenceState.SAFE:
|
||||
self.led_pub.publish(String(data="off"))
|
||||
self.returning_home = False
|
||||
elif self.current_state == GeofenceState.WARNING:
|
||||
self.led_pub.publish(String(data="amber"))
|
||||
self.emotion_pub.publish(String(data="concerned"))
|
||||
if self.tts_enabled:
|
||||
msg = self.get_parameter("tts.warning_message").value
|
||||
self.tts_pub.publish(String(data=msg))
|
||||
elif self.current_state == GeofenceState.VIOLATION:
|
||||
self.led_pub.publish(String(data="red"))
|
||||
self.emotion_pub.publish(String(data="concerned"))
|
||||
if self.tts_enabled:
|
||||
msg = self.get_parameter("tts.violation_message").value
|
||||
self.tts_pub.publish(String(data=msg))
|
||||
twist = Twist()
|
||||
self.cmd_vel_pub.publish(twist)
|
||||
if self.auto_return_enabled and not self.returning_home and self.home_lat:
|
||||
self.returning_home = True
|
||||
self._initiate_auto_return()
|
||||
self.last_state = self.current_state
|
||||
|
||||
def _point_in_polygon(self, point: Tuple[float, float], vertices: List[Tuple[float, float]]) -> bool:
|
||||
"""Ray casting algorithm for point-in-polygon test."""
|
||||
x, y = point
|
||||
n = len(vertices)
|
||||
inside = False
|
||||
def _initiate_auto_return(self):
|
||||
twist = Twist()
|
||||
twist.linear.x = self.auto_return_speed
|
||||
self.cmd_vel_pub.publish(twist)
|
||||
|
||||
p1x, p1y = vertices[0]
|
||||
for i in range(1, n + 1):
|
||||
p2x, p2y = vertices[i % n]
|
||||
@staticmethod
|
||||
def _haversine_distance(lat1, lon1, lat2, lon2):
|
||||
R = 6371000
|
||||
phi1, phi2 = math.radians(lat1), math.radians(lat2)
|
||||
delta_phi = math.radians(lat2 - lat1)
|
||||
delta_lambda = math.radians(lon2 - lon1)
|
||||
a = math.sin(delta_phi/2)**2 + math.cos(phi1) * math.cos(phi2) * math.sin(delta_lambda/2)**2
|
||||
return R * 2 * math.asin(math.sqrt(a))
|
||||
|
||||
# Check if ray crosses edge
|
||||
if y > min(p1y, p2y):
|
||||
if y <= max(p1y, p2y):
|
||||
if x <= max(p1x, p2x):
|
||||
if p1y != p2y:
|
||||
xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x
|
||||
if p1x == p2x or x <= xinters:
|
||||
@staticmethod
|
||||
def _point_in_polygon(lat, lon, polygon):
|
||||
if not polygon or len(polygon) < 3:
|
||||
return False
|
||||
inside, p1_lat, p1_lon = False, polygon[0][0], polygon[0][1]
|
||||
for i in range(1, len(polygon) + 1):
|
||||
p2_lat, p2_lon = polygon[i % len(polygon)]
|
||||
if lat > min(p1_lat, p2_lat) and lat <= max(p1_lat, p2_lat) and lon <= max(p1_lon, p2_lon):
|
||||
if p1_lat != p2_lat:
|
||||
xinters = (lat - p1_lat) * (p2_lon - p1_lon) / (p2_lat - p1_lat) + p1_lon
|
||||
if p1_lat == p2_lat or lon <= xinters:
|
||||
inside = not inside
|
||||
|
||||
p1x, p1y = p2x, p2y
|
||||
|
||||
p1_lat, p1_lon = p2_lat, p2_lon
|
||||
return inside
|
||||
|
||||
@staticmethod
|
||||
def _min_distance_to_polygon(lat, lon, polygon):
|
||||
if not polygon or len(polygon) < 2:
|
||||
return float("inf")
|
||||
min_dist = float("inf")
|
||||
for i in range(len(polygon)):
|
||||
p1, p2 = polygon[i], polygon[(i + 1) % len(polygon)]
|
||||
d = abs((p2[0]-p1[0])*(p1[1]-lon) - (p1[0]-lat)*(p2[1]-p1[1]))
|
||||
d /= max(math.sqrt((p2[0]-p1[0])**2 + (p2[1]-p1[1])**2), 1e-6)
|
||||
min_dist = min(min_dist, d)
|
||||
return min_dist
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = GeofenceNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
rclpy.spin(GeofenceNode())
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user