<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypes="xml"?>
<package format="3">
  <name>vision_rescue_bot</name>
  <version>0.0.0</version>
  <description>Vision processing for rescue robot - iPBL competition</description>
  <maintainer email="your.email@example.com">Your Name</maintainer>
  <license>Apache-2.0</license>
 
  <depend>rclpy</depend>
  <depend>sensor_msgs</depend>
  <depend>std_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>cv_bridge</depend>
  
  <exec_depend>python3-opencv</exec_depend>
  <exec_depend>python3-numpy</exec_depend>
 
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
 
  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

from setuptools import setup
 
package_name = 'vision_rescue_bot'
 
setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Your Name',
    maintainer_email='your.email@example.com',
    description='Vision processing for rescue robot',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'vision_node = vision_rescue_bot.vision_node:main',
            'camera_simulator = vision_rescue_bot.camera_simulator:main',
            'mission_scenarios = vision_rescue_bot.mission_scenarios:main',
        ],
    },
)

#!/usr/bin/env python3
"""
Mission-Specific Vision Node for Rescue Robot
Detects victims, classifies urgency, reports coordinates to HQ
"""
 
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import String
from geometry_msgs.msg import Point
from cv_bridge import CvBridge
import cv2
import numpy as np
import json
from datetime import datetime
 
class VisionRescueNode(Node):
    def __init__(self):
        super().__init__('vision_rescue_node')
        
        # CV Bridge for ROS2 ↔ OpenCV conversion
        self.bridge = CvBridge()
        
        # Subscribers
        self.image_subscription = self.create_subscription(
            Image, '/camera/image_raw', self.image_callback, 10)
        
        # Publishers for mission-specific data
        self.victim_report_pub = self.create_publisher(
            String, '/rescue/victim_report', 10)
        self.coordinates_pub = self.create_publisher(
            Point, '/rescue/victim_coordinates', 10)
        self.status_pub = self.create_publisher(
            String, '/rescue/status', 10)
        
        # Mission parameters
        self.victim_id_counter = 0
        self.detection_threshold = 100
        self.robot_position = {"x": 0, "y": 0}  # Updated by navigation system
        
        # Warehouse coordinate system (3m x 2m warehouse)
        self.warehouse_width = 3.0   # meters
        self.warehouse_height = 2.0  # meters
        self.image_width = 640
        self.image_height = 480
        
        self.get_logger().info('🚨 Rescue Vision System ONLINE - Ready for victim detection!')
        self.publish_status("SYSTEM_READY", "Vision system initialized for rescue mission")
 
    def image_callback(self, msg):
        try:
            # Convert ROS2 image to OpenCV
            cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
            
            # Enhance for dark warehouse conditions
            enhanced_image = self.enhance_for_dark_conditions(cv_image)
            
            # Detect potential victims
            victims = self.detect_victims(enhanced_image)
            
            # Process each detected victim
            for victim in victims:
                self.process_victim_detection(victim, enhanced_image)
                
        except Exception as e:
            self.get_logger().error(f'Vision processing error: {str(e)}')
            self.publish_status("ERROR", f"Processing failed: {str(e)}")
 
    def enhance_for_dark_conditions(self, cv_image):
        """Enhance image visibility for dark warehouse zones"""
        # Convert to LAB color space for better brightness control
        lab = cv2.cvtColor(cv_image, cv2.COLOR_BGR2LAB)
        l, a, b = cv2.split(lab)
        
        # Apply CLAHE (Contrast Limited Adaptive Histogram Equalization)
        clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8,8))
        l = clahe.apply(l)
        
        # Merge back and convert to BGR
        enhanced = cv2.merge([l, a, b])
        enhanced = cv2.cvtColor(enhanced, cv2.COLOR_LAB2BGR)
        
        return enhanced
 
    def detect_victims(self, cv_image):
        """Detect potential victims using color and shape analysis"""
        # Convert to HSV for better color detection
        hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        
        # Expanded color ranges for victim detection (clothing + skin)
        color_ranges = [
            # Human skin tones
            ([0, 20, 70], [20, 255, 255]),
            # Common clothing colors that stand out in debris
            ([100, 50, 50], [130, 255, 255]),  # Blue clothing
            ([0, 50, 50], [10, 255, 255]),     # Red clothing
        ]
        
        all_victims = []
        
        for lower, upper in color_ranges:
            mask = cv2.inRange(hsv, np.array(lower), np.array(upper))
            contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            
            for contour in contours:
                area = cv2.contourArea(contour)
                if area > self.detection_threshold:
                    # Calculate bounding box and center
                    x, y, w, h = cv2.boundingRect(contour)
                    center_x = x + w // 2
                    center_y = y + h // 2
                    
                    victim_data = {
                        'contour': contour,
                        'area': area,
                        'bounding_box': (x, y, w, h),
                        'center': (center_x, center_y),
                        'detection_type': 'color_based'
                    }
                    all_victims.append(victim_data)
        
        return all_victims
 
    def calculate_victim_visibility(self, victim_data, cv_image):
        """Calculate what percentage of victim is visible (mission critical)"""
        x, y, w, h = victim_data['bounding_box']
        
        # Extract victim region
        victim_region = cv_image[y:y+h, x:x+w]
        
        # Convert to grayscale for analysis
        gray = cv2.cvtColor(victim_region, cv2.COLOR_BGR2GRAY)
        
        # Calculate visibility based on non-debris pixels
        # This is simplified - real implementation would use ML
        total_pixels = w * h
        # Assume darker pixels are debris/rubble covering victim
        visible_pixels = np.sum(gray > 80)  # Threshold for "visible" pixels
        
        visibility_percent = (visible_pixels / total_pixels) * 100
        return min(visibility_percent, 100.0)
 
    def classify_victim_urgency(self, visibility_percent):
        """Classify victim urgency based on visibility (mission requirement)"""
        if visibility_percent > 50:
            return "LIGHT_INJURY", "GREEN", "Low priority - victim largely visible"
        elif visibility_percent > 10:
            return "MEDIUM_INJURY", "YELLOW", "Medium priority - partially buried"
        else:
            return "CRITICAL", "RED", "⚠️  IMMEDIATE HELP NEEDED - victim mostly buried"
 
    def pixel_to_warehouse_coords(self, pixel_x, pixel_y):
        """Convert pixel coordinates to real warehouse coordinates"""
        # Convert from image coordinates to warehouse meters
        warehouse_x = (pixel_x / self.image_width) * self.warehouse_width
        warehouse_y = (pixel_y / self.image_height) * self.warehouse_height
        
        return {"x": round(warehouse_x, 2), "y": round(warehouse_y, 2)}
 
    def process_victim_detection(self, victim_data, cv_image):
        """Process detected victim and report to HQ"""
        # Calculate victim visibility
        visibility = self.calculate_victim_visibility(victim_data, cv_image)
        
        # Classify urgency level
        urgency, priority_color, description = self.classify_victim_urgency(visibility)
        
        # Generate unique victim ID
        self.victim_id_counter += 1
        victim_id = f"VICTIM_{self.victim_id_counter:03d}"
        
        # Convert to warehouse coordinates
        pixel_x, pixel_y = victim_data['center']
        warehouse_coords = self.pixel_to_warehouse_coords(pixel_x, pixel_y)
        
        # Create comprehensive victim report
        victim_report = {
            "victim_id": victim_id,
            "timestamp": datetime.now().isoformat(),
            "coordinates": warehouse_coords,
            "urgency_level": urgency,
            "priority_color": priority_color,
            "visibility_percent": round(visibility, 1),
            "description": description,
            "detection_area": victim_data['area'],
            "robot_position": self.robot_position
        }
        
        # Publish to rescue HQ
        self.publish_victim_report(victim_report)
        self.publish_coordinates(warehouse_coords)
        
        # Log for operators
        self.get_logger().info(
            f"🆘 {victim_id} detected at ({warehouse_coords['x']}, {warehouse_coords['y']}) "
            f"- {urgency} ({visibility:.1f}% visible)"
        )
 
    def publish_victim_report(self, report):
        """Send victim report to rescue headquarters"""
        report_msg = String()
        report_msg.data = json.dumps(report)
        self.victim_report_pub.publish(report_msg)
 
    def publish_coordinates(self, coords):
        """Publish victim coordinates for navigation system"""
        point_msg = Point()
        point_msg.x = coords['x']
        point_msg.y = coords['y']
        point_msg.z = 0.0  # Ground level
        self.coordinates_pub.publish(point_msg)
 
    def publish_status(self, status_type, message):
        """Publish system status updates"""
        status_msg = String()
        status_data = {
            "type": status_type,
            "message": message,
            "timestamp": datetime.now().isoformat()
        }
        status_msg.data = json.dumps(status_data)
        self.status_pub.publish(status_msg)
 
def main(args=None):
    rclpy.init(args=args)
    vision_node = VisionRescueNode()
    
    try:
        rclpy.spin(vision_node)
    except KeyboardInterrupt:
        pass
    finally:
        vision_node.destroy_node()
        rclpy.shutdown()
 
if __name__ == '__main__':
    main()

#!/usr/bin/env python3
"""
Mission Scenario Simulator for Rescue Robot Testing
Simulates realistic warehouse collapse scenarios
"""
 
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
 
class MissionScenarioSimulator(Node):
    def __init__(self):
        super().__init__('mission_scenario_simulator')
        
        self.image_publisher = self.create_publisher(Image, '/camera/image_raw', 10)
        self.timer = self.create_timer(0.1, self.publish_scenario_image)  # 10 FPS
        
        self.bridge = CvBridge()
        self.frame_counter = 0
        self.scenario_duration = 100  # frames per scenario
        
        # Mission-specific scenarios
        self.scenarios = [
            'empty_warehouse',
            'light_injury_victim',     # >50% visible
            'medium_injury_victim',    # <50% visible
            'critical_victim_buried',  # <10% visible - URGENT
            'dark_zone_navigation',
            'multiple_victims',
            'debris_false_positive'
        ]
        
        self.get_logger().info('🎬 Mission Scenario Simulator started - Testing rescue scenarios')
 
    def publish_scenario_image(self):
        """Generate mission-realistic test scenarios"""
        scenario_index = (self.frame_counter // self.scenario_duration) % len(self.scenarios)
        current_scenario = self.scenarios[scenario_index]
        
        # Generate scenario-specific image
        test_image = self.create_scenario_image(current_scenario)
        
        # Add mission info overlay
        self.add_mission_overlay(test_image, current_scenario)
        
        # Convert and publish
        try:
            image_msg = self.bridge.cv2_to_imgmsg(test_image, 'bgr8')
            image_msg.header.stamp = self.get_clock().now().to_msg()
            image_msg.header.frame_id = 'rescue_camera'
            
            self.image_publisher.publish(image_msg)
            
        except Exception as e:
            self.get_logger().error(f'Simulator error: {str(e)}')
        
        self.frame_counter += 1
 
    def create_scenario_image(self, scenario):
        """Create scenario-specific test images"""
        height, width = 480, 640
        
        if scenario == 'empty_warehouse':
            # Empty collapsed warehouse section
            image = np.ones((height, width, 3), dtype=np.uint8) * 50  # Dark gray
            # Add some debris texture
            noise = np.random.randint(0, 30, (height, width, 3))
            image = cv2.add(image, noise.astype(np.uint8))
            
        elif scenario == 'light_injury_victim':
            # Victim >50% visible (Low priority)
            image = self.create_debris_background()
            # Add mostly visible person (skin + clothing)
            cv2.rectangle(image, (200, 150), (450, 400), (132, 165, 210), -1)  # Large visible area
            cv2.rectangle(image, (220, 170), (430, 250), (60, 90, 150), -1)   # Blue clothing
            
        elif scenario == 'medium_injury_victim':
            # Victim <50% visible (Medium priority)
            image = self.create_debris_background()
            # Add partially buried person
            cv2.rectangle(image, (300, 200), (400, 350), (132, 165, 210), -1)  # Smaller visible area
            # Add covering debris
            cv2.rectangle(image, (290, 180), (420, 280), (40, 40, 40), -1)
            
        elif scenario == 'critical_victim_buried':
            # Victim <10% visible (CRITICAL - immediate help)
            image = self.create_debris_background()
            # Add barely visible person (just small exposed area)
            cv2.circle(image, (350, 300), 25, (132, 165, 210), -1)  # Just face/hand visible
            # Heavy debris coverage
            cv2.rectangle(image, (320, 280), (450, 400), (30, 30, 30), -1)
            
        elif scenario == 'dark_zone_navigation':
            # Simulate dark warehouse area
            image = np.ones((height, width, 3), dtype=np.uint8) * 20  # Very dark
            # Add victim in low light
            cv2.rectangle(image, (250, 200), (390, 350), (80, 90, 100), -1)
            
        elif scenario == 'multiple_victims':
            # Multiple victims at different urgency levels
            image = self.create_debris_background()
            # Light injury victim
            cv2.rectangle(image, (100, 150), (200, 300), (132, 165, 210), -1)
            # Critical victim
            cv2.circle(image, (500, 350), 20, (132, 165, 210), -1)
            
        elif scenario == 'debris_false_positive':
            # Test false positive handling
            image = self.create_debris_background()
            # Add skin-colored debris (not human)
            cv2.rectangle(image, (300, 250), (350, 300), (132, 165, 210), -1)
            
        else:
            image = self.create_debris_background()
        
        return image
 
    def create_debris_background(self):
        """Create realistic warehouse debris background"""
        height, width = 480, 640
        # Start with dark concrete
        image = np.ones((height, width, 3), dtype=np.uint8) * 60
        
        # Add random debris shapes
        for _ in range(20):
            x1, y1 = np.random.randint(0, width-50), np.random.randint(0, height-50)
            x2, y2 = x1 + np.random.randint(20, 80), y1 + np.random.randint(20, 80)
            color = tuple(np.random.randint(20, 100, 3).tolist())
            cv2.rectangle(image, (x1, y1), (x2, y2), color, -1)
        
        # Add texture and noise
        noise = np.random.randint(-20, 20, (height, width, 3))
        image = cv2.add(image, noise.astype(np.uint8))
        
        return image
 
    def add_mission_overlay(self, image, scenario):
        """Add mission information overlay"""
        # Add scenario label
        cv2.putText(image, f'SCENARIO: {scenario.upper()}', 
                   (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
        
        # Add frame counter
        cv2.putText(image, f'Frame: {self.frame_counter}', 
                   (10, 460), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
        
        # Add mission-specific indicators
        if 'critical' in scenario:
            cv2.putText(image, '⚠️  CRITICAL VICTIM - IMMEDIATE HELP NEEDED', 
                       (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
        elif 'light' in scenario:
            cv2.putText(image, '✅ Light Injury - Low Priority', 
                       (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
        elif 'medium' in scenario:
            cv2.putText(image, '⚠️  Medium Injury - Rescue Needed', 
                       (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 165, 255), 2)
 
def main(args=None):
    rclpy.init(args=args)
    simulator = MissionScenarioSimulator()
    
    try:
        rclpy.spin(simulator)
    except KeyboardInterrupt:
        pass
    finally:
        simulator.destroy_node()
        rclpy.shutdown()
 
if __name__ == '__main__':
    main()

# Create workspace
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
 
# Create package
ros2 pkg create --build-type ament_python vision_rescue_bot \
    --dependencies rclpy sensor_msgs cv_bridge opencv-python std_msgs geometry_msgs
 
# Copy all files above into the package structure
 
# Build
cd ~/ros2_ws
colcon build --packages-select vision_rescue_bot
source install/setup.bash

# Terminal 1 - Main vision processing
ros2 run vision_rescue_bot vision_node
 
# Terminal 2 - Mission scenario simulator  
ros2 run vision_rescue_bot mission_scenarios
 
# Terminal 3 - Monitor victim reports
ros2 topic echo /rescue/victim_report
 
# Terminal 4 - Monitor coordinates
ros2 topic echo /rescue/victim_coordinates

# Check all rescue-related topics
ros2 topic list | grep rescue
 
# View victim detection data
ros2 topic echo /rescue/victim_report | jq '.'
 
# Monitor system status
ros2 topic echo /rescue/status

# Method 1: Check your router's admin page
# Go to your router's web interface (usually 192.168.1.1 or 192.168.0.1)
# Look for "Connected Devices" or "DHCP Clients"
# Find device named "ubuntu" or with MAC starting with DC:A6:32
 
# Method 2: Network scan (install nmap first)
brew install nmap
nmap -sn 192.168.1.0/24  # Adjust IP range for your network
 
# Method 3: Connect monitor and keyboard to Pi
# Look for IP address in boot messages or run: ip addr show