Sai Jagadeesh Muralikrishnan

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python camera_calibration
cd camera_calibration

sudo apt install ros-${ROS_DISTRO}-cv-bridge ros-${ROS_DISTRO}-image-transport
pip install opencv-python numpy

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
import os
from datetime import datetime

class CameraCalibrationNode(Node):
    def __init__(self):
        super().__init__('camera_calibration_node')
        
        # Parameters
        self.declare_parameter('chessboard_width', 9)
        self.declare_parameter('chessboard_height', 6)
        self.declare_parameter('square_size', 0.024)  # meters
        self.declare_parameter('image_topic', '/camera/image_raw')
        
        # Initialize variables
        self.chessboard_size = (
            self.get_parameter('chessboard_width').value,
            self.get_parameter('chessboard_height').value)
        self.square_size = self.get_parameter('square_size').value
        
        self.bridge = CvBridge()
        self.image_sub = self.create_subscription(
            Image,
            self.get_parameter('image_topic').value,
            self.image_callback,
            10)
        
        # Calibration variables
        self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        self.objp = np.zeros((self.chessboard_size[0]*self.chessboard_size[1],3), np.float32)
        self.objp[:,:2] = np.mgrid[0:self.chessboard_size[0],0:self.chessboard_size[1]].T.reshape(-1,2) * self.square_size
        
        self.objpoints = []  # 3D points in real world space
        self.imgpoints = []  # 2D points in image plane
        self.calibration_done = False
    
    def image_callback(self, msg):
        if self.calibration_done:
            return
            
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
            gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
            
            # Find chessboard corners
            ret, corners = cv2.findChessboardCorners(
                gray, self.chessboard_size, 
                cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE)
            
            if ret:
                self.get_logger().info('Chessboard detected!')
                
                # Refine corner locations
                corners2 = cv2.cornerSubPix(
                    gray, corners, (11,11), (-1,-1), self.criteria)
                
                # Draw and display the corners
                cv2.drawChessboardCorners(cv_image, self.chessboard_size, corners2, ret)
                cv2.imshow('Calibration', cv_image)
                cv2.waitKey(500)  # Display for 500ms
                
                # Add to calibration data
                self.objpoints.append(self.objp)
                self.imgpoints.append(corners2)
                
                if len(self.objpoints) >= 15:  # Use at least 15 images
                    self.calibrate_camera(gray.shape[::-1])
                    
        except Exception as e:
            self.get_logger().error(f'Error processing image: {str(e)}')
    
    def calibrate_camera(self, image_size):
        self.get_logger().info('Starting calibration...')
        
        try:
            ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
                self.objpoints, self.imgpoints, image_size, None, None)
            
            if ret:
                self.get_logger().info('Calibration successful!')
                self.save_calibration(mtx, dist)
                self.calibration_done = True
                
        except Exception as e:
            self.get_logger().error(f'Calibration failed: {str(e)}')
    
    def save_calibration(self, camera_matrix, distortion_coeffs):
        timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
        filename = f'camera_calibration_{timestamp}.npz'
        
        np.savez(filename, 
                camera_matrix=camera_matrix,
                distortion_coeffs=distortion_coeffs)
        
        self.get_logger().info(f'Saved calibration to {filename}')

def main(args=None):
    rclpy.init(args=args)
    node = CameraCalibrationNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='camera_calibration',
            executable='calibration_node',
            name='camera_calibration',
            parameters=[{
                'chessboard_width': 9,
                'chessboard_height': 6,
                'square_size': 0.024,
                'image_topic': '/camera/image_raw'
            }]
        )
    ])

# Add this to setup.py if not present
entry_points={
    'console_scripts': [
        'calibration_node = camera_calibration.calibration_node:main',
    ],
},

# Build and run
cd ~/ros2_ws
colcon build --packages-select camera_calibration
source install/setup.bash
ros2 launch camera_calibration calibration.launch.py

#!/bin/bash

# Exit on error
set -e

# Define build type (dev or release)
if [ "$1" == "--release" ]; then
    BUILD_TYPE="Release"
    BUILD_FLAGS="--cmake-args -DCMAKE_BUILD_TYPE=Release -DNDEBUG=ON"
    echo "Building in Release mode"
else
    BUILD_TYPE="Debug"
    BUILD_FLAGS="--cmake-args -DCMAKE_BUILD_TYPE=Debug"
    echo "Building in Debug mode"
fi

# Clear previous build
if [ "$2" == "--clean" ]; then
    echo "Cleaning previous build..."
    rm -rf build install log
fi

# Source ROS setup
if [ -z "$ROS_DISTRO" ]; then
    echo "ROS environment not set, sourcing..."
    if [ -f "/opt/ros/humble/setup.bash" ]; then
        source /opt/ros/humble/setup.bash
    else
        echo "ERROR: ROS setup file not found. Update the path or install ROS."
        exit 1
    fi
fi

# Find the package directory
PKG_DIR=$(pwd)
while [ "$PKG_DIR" != "/" ] && [ ! -f "$PKG_DIR/COLCON_IGNORE" ] && [ ! -f "$PKG_DIR/package.xml" ]; do
    PKG_DIR=$(dirname "$PKG_DIR")
done

if [ -f "$PKG_DIR/package.xml" ]; then
    echo "Building ${PKG_DIR##*/} package in $BUILD_TYPE mode"
    cd "$PKG_DIR/../.." || exit  # Move to workspace root
else
    echo "No package.xml found in any parent directory."
    echo "Please run this script from within your ROS2 package directory."
    exit 1
fi

# Build commands
colcon build \
    $BUILD_FLAGS \
    --symlink-install \
    --event-handlers console_direct+ \
    --packages-select "${PKG_DIR##*/}" \
    --parallel-workers $(nproc)

echo "Build complete. Source the workspace with:"
echo "source ./install/setup.bash"

#!/bin/bash

if [ "$#" -ne 4 ]; then
    echo "Usage: $0 <input_bag> <image_topic> <output_video> <fps>"
    exit 1
fi

INPUT_BAG=$1
IMAGE_TOPIC=$2
OUTPUT_VIDEO=$3
FPS=$4

# Create temp directory
TEMP_DIR=$(mktemp -d)
echo "Created temp directory: $TEMP_DIR"

# Extract images from bag file
echo "Extracting images from bag file..."
ros2 run image_view extract_images _sec_per_frame:=0 _filename_format:="$TEMP_DIR/frame_%04d.jpg" __name:=extract_images &
EXTRACT_PID=$!

ros2 bag play "$INPUT_BAG" --rate 1.0 --topics "$IMAGE_TOPIC" --remap "$IMAGE_TOPIC:=/extract_images/image"
kill $EXTRACT_PID

# Convert images to video
echo "Converting images to video..."
ffmpeg -y -r "$FPS" -i "$TEMP_DIR/frame_%04d.jpg" -c:v libx264 -vf "fps=$FPS,format=yuv420p" "$OUTPUT_VIDEO"

# Clean up
rm -r "$TEMP_DIR"
echo "Done. Output video saved to $OUTPUT_VIDEO"