jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py

@@ -1,131 +1,179 @@
 #!/usr/bin/env python3
-"""Geofence boundary enforcer for SaltyBot.
+"""Geofence safety system for SaltyBot.
 
-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
+Subscribes to /phone/gps and /odom. Enforces circle/polygon geofence.
+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 initialized: type={self.geofence_type}")
 
-        # 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.get_logger().info(f"Home: ({self.home_lat:.6f}, {self.home_lon:.6f})")
+        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
-        )
-
-        # 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})"
+    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
             )
-            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 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
 
-        msg_breach = Bool(data=breach)
-        self.pub_breach.publish(msg_breach)
+        if new_state != self.current_state:
+            self.current_state = new_state
+            self._on_state_change()
 
-    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)
+    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:
+                self.tts_pub.publish(String(data=self.get_parameter("tts.warning_message").value))
+        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))
+            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 _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)
+
+    @staticmethod
+    def _haversine_distance(lat1, lon1, lat2, lon2):
+        R = 6371000
+        phi1 = math.radians(lat1)
+        phi2 = 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
+
+    @staticmethod
+    def _point_in_polygon(lat, lon, polygon):
+        if not polygon or len(polygon) < 3:
+            return False
+        n = len(polygon)
         inside = False
-
-        p1x, p1y = vertices[0]
+        p1_lat, p1_lon = polygon[0]
         for i in range(1, n + 1):
-            p2x, p2y = vertices[i % n]
-
-            # 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:
+            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
-
-            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 = 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)
+            min_dist = min(min_dist, d)
+        return min_dist
 
 def main(args=None):
     rclpy.init(args=args)
@@ -138,6 +186,5 @@ def main(args=None):
         node.destroy_node()
         rclpy.shutdown()
 
-
 if __name__ == "__main__":
     main()