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

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

@ -1,7 +1,7 @@
#!/usr/bin/env python3 #!/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. Three zones: SAFE, WARNING (2m buffer), VIOLATION.
WARNING: slow + concern face + TTS + amber LED. WARNING: slow + concern face + TTS + amber LED.
VIOLATION: stop + auto-return + red LED. VIOLATION: stop + auto-return + red LED.
@ -62,7 +62,7 @@ class GeofenceNode(Node):
self.returning_home = False self.returning_home = False
self.home_lat = None self.home_lat = None
self.home_lon = 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): def _gps_callback(self, msg: NavSatFix):
if msg.latitude == 0 and msg.longitude == 0: if msg.latitude == 0 and msg.longitude == 0:
@ -72,7 +72,6 @@ class GeofenceNode(Node):
if self.home_lat is None: if self.home_lat is None:
self.home_lat = msg.latitude self.home_lat = msg.latitude
self.home_lon = msg.longitude self.home_lon = msg.longitude
self.get_logger().info(f"Home: ({self.home_lat:.6f}, {self.home_lon:.6f})")
self._update_geofence_state() self._update_geofence_state()
def _odom_callback(self, msg: Odometry): def _odom_callback(self, msg: Odometry):
@ -80,10 +79,7 @@ class GeofenceNode(Node):
def _update_geofence_state(self): def _update_geofence_state(self):
if self.geofence_type == "circle": if self.geofence_type == "circle":
distance = self._haversine_distance( distance = self._haversine_distance(self.current_lat, self.current_lon, self.circle_center_lat, self.circle_center_lon)
self.current_lat, self.current_lon,
self.circle_center_lat, self.circle_center_lon
)
if distance <= self.circle_radius - self.warning_distance: if distance <= self.circle_radius - self.warning_distance:
new_state = GeofenceState.SAFE new_state = GeofenceState.SAFE
elif distance <= self.circle_radius: elif distance <= self.circle_radius:
@ -113,12 +109,14 @@ class GeofenceNode(Node):
self.led_pub.publish(String(data="amber")) self.led_pub.publish(String(data="amber"))
self.emotion_pub.publish(String(data="concerned")) self.emotion_pub.publish(String(data="concerned"))
if self.tts_enabled: 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: elif self.current_state == GeofenceState.VIOLATION:
self.led_pub.publish(String(data="red")) self.led_pub.publish(String(data="red"))
self.emotion_pub.publish(String(data="concerned")) self.emotion_pub.publish(String(data="concerned"))
if self.tts_enabled: 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() twist = Twist()
self.cmd_vel_pub.publish(twist) self.cmd_vel_pub.publish(twist)
if self.auto_return_enabled and not self.returning_home and self.home_lat: 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 self.last_state = self.current_state
def _initiate_auto_return(self): def _initiate_auto_return(self):
self.get_logger().warn("Auto-return initiated")
twist = Twist() twist = Twist()
twist.linear.x = self.auto_return_speed twist.linear.x = self.auto_return_speed
self.cmd_vel_pub.publish(twist) self.cmd_vel_pub.publish(twist)
@ -135,26 +132,20 @@ class GeofenceNode(Node):
@staticmethod @staticmethod
def _haversine_distance(lat1, lon1, lat2, lon2): def _haversine_distance(lat1, lon1, lat2, lon2):
R = 6371000 R = 6371000
phi1 = math.radians(lat1) phi1, phi2 = math.radians(lat1), math.radians(lat2)
phi2 = math.radians(lat2)
delta_phi = math.radians(lat2 - lat1) delta_phi = math.radians(lat2 - lat1)
delta_lambda = math.radians(lon2 - lon1) 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 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 * 2 * math.asin(math.sqrt(a))
return R * c
@staticmethod @staticmethod
def _point_in_polygon(lat, lon, polygon): def _point_in_polygon(lat, lon, polygon):
if not polygon or len(polygon) < 3: if not polygon or len(polygon) < 3:
return False return False
n = len(polygon) inside, p1_lat, p1_lon = False, polygon[0][0], polygon[0][1]
inside = False for i in range(1, len(polygon) + 1):
p1_lat, p1_lon = polygon[0] p2_lat, p2_lon = polygon[i % len(polygon)]
for i in range(1, n + 1): if lat > min(p1_lat, p2_lat) and lat <= max(p1_lat, p2_lat) and lon <= max(p1_lon, p2_lon):
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):
if p1_lat != p2_lat: if p1_lat != p2_lat:
xinters = (lat - p1_lat) * (p2_lon - p1_lon) / (p2_lat - p1_lat) + p1_lon xinters = (lat - p1_lat) * (p2_lon - p1_lon) / (p2_lat - p1_lat) + p1_lon
if p1_lat == p2_lat or lon <= xinters: if p1_lat == p2_lat or lon <= xinters:
@ -168,22 +159,19 @@ class GeofenceNode(Node):
return float("inf") return float("inf")
min_dist = float("inf") min_dist = float("inf")
for i in range(len(polygon)): for i in range(len(polygon)):
p1 = polygon[i] p1, p2 = polygon[i], polygon[(i + 1) % len(polygon)]
p2 = polygon[(i + 1) % len(polygon)] d = abs((p2[0]-p1[0])*(p1[1]-lon) - (p1[0]-lat)*(p2[1]-p1[1]))
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)
d /= max(math.sqrt((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2), 1e-6)
min_dist = min(min_dist, d) min_dist = min(min_dist, d)
return min_dist return min_dist
def main(args=None): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
node = GeofenceNode()
try: try:
rclpy.spin(node) rclpy.spin(GeofenceNode())
except KeyboardInterrupt: except KeyboardInterrupt:
pass pass
finally: finally:
node.destroy_node()
rclpy.shutdown() rclpy.shutdown()
if __name__ == "__main__": if __name__ == "__main__":