seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/sl-controls/issue-441-geofence'

Browse Source 
# Conflicts:
#	jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py
This commit is contained in:
sl-jetson 2026-03-05 09:10:17 -05:00
commit 3d008ddbb7
1 changed files with 23 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python3
"""Geofence safety system for SaltyBot.
"""Geofence safety system for SaltyBot - Issue #441.
Subscribes to /phone/gps and /odom. Enforces circle/polygon geofence.
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.
@ -62,7 +62,7 @@ class GeofenceNode(Node):
self.returning_home = False
self.home_lat = None
self.home_lon = None
self.get_logger().info(f"Geofence initialized: type={self.geofence_type}")
self.get_logger().info(f"Geofence: type={self.geofence_type}, radius={self.circle_radius if self.geofence_type=='circle' else 'polygon'}m")
def _gps_callback(self, msg: NavSatFix):
if msg.latitude == 0 and msg.longitude == 0:
@ -72,7 +72,6 @@ class GeofenceNode(Node):
if self.home_lat is None:
self.home_lat = msg.latitude
self.home_lon = msg.longitude
self.get_logger().info(f"Home: ({self.home_lat:.6f}, {self.home_lon:.6f})")
self._update_geofence_state()
def _odom_callback(self, msg: Odometry):
@ -80,10 +79,7 @@ class GeofenceNode(Node):
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
)
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:
@ -113,12 +109,14 @@ class GeofenceNode(Node):
self.led_pub.publish(String(data="amber"))
self.emotion_pub.publish(String(data="concerned"))
if self.tts_enabled:
self.tts_pub.publish(String(data=self.get_parameter("tts.warning_message").value))
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:
self.tts_pub.publish(String(data=self.get_parameter("tts.violation_message").value))
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:
@ -127,7 +125,6 @@ class GeofenceNode(Node):
self.last_state = self.current_state
def _initiate_auto_return(self):
self.get_logger().warn("Auto-return initiated")
twist = Twist()
twist.linear.x = self.auto_return_speed
self.cmd_vel_pub.publish(twist)
@ -135,26 +132,20 @@ class GeofenceNode(Node):
@staticmethod
def _haversine_distance(lat1, lon1, lat2, lon2):
R = 6371000
phi1 = math.radians(lat1)
phi2 = math.radians(lat2)
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
c = 2 * math.asin(math.sqrt(a))
return R * c
return R * 2 * math.asin(math.sqrt(a))
@staticmethod
def _point_in_polygon(lat, lon, polygon):
if not polygon or len(polygon) < 3:
return False
n = len(polygon)
inside = False
p1_lat, p1_lon = polygon[0]
for i in range(1, n + 1):
p2_lat, p2_lon = polygon[i % n]
if lat > min(p1_lat, p2_lat):
if lat <= max(p1_lat, p2_lat):
if lon <= max(p1_lon, p2_lon):
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:
@ -168,8 +159,7 @@ class GeofenceNode(Node):
return float("inf")
min_dist = float("inf")
for i in range(len(polygon)):
p1 = polygon[i]
p2 = polygon[(i + 1) % 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)
@ -177,13 +167,11 @@ class GeofenceNode(Node):
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__":