diff --git a/jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py b/jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py index 88dee35..ffd2b94 100644 --- a/jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py +++ b/jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py @@ -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,30 +132,24 @@ 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 + 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)) @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): - 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 + 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 p1_lat, p1_lon = p2_lat, p2_lon return inside @@ -168,22 +159,19 @@ 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)] - 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) + 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__":