# Step 1: Full system update
$ sudo apt update && sudo apt upgrade -y

# Step 2: Enable I2C and camera interfaces
$ sudo raspi-config
# Navigate: Interface Options → I2C → Enable
# Navigate: Interface Options → Camera → Enable
# Also enable: Interface Options → SSH (for remote access)
# Select "Finish" and reboot when prompted

# Step 3: Add your user to I2C and GPIO groups
$ sudo usermod -aG i2c,gpio,video $USER

# Step 4: Install I2C tools and verify hardware
$ sudo apt install -y i2c-tools python3-smbus
$ i2cdetect -y 1
# You should see the TurboPi expansion board at address 0x7f (or 0x70)

# Step 5: Increase swap space to 2 GB
$ sudo dphys-swapfile swapoff
$ sudo sed -i 's/CONF_SWAPSIZE=100/CONF_SWAPSIZE=2048/' /etc/dphys-swapfile
$ sudo dphys-swapfile setup
$ sudo dphys-swapfile swapon

# Step 6: Verify swap is active
$ free -h
# Should show ~2G under Swap

# Remove any old Docker installations
$ sudo apt remove -y docker docker-engine docker.io containerd runc

# Install prerequisites
$ sudo apt update
$ sudo apt install -y ca-certificates curl gnupg lsb-release

# Add Docker's official GPG key
$ sudo install -m 0755 -d /etc/apt/keyrings
$ sudo curl -fsSL https://download.docker.com/linux/debian/gpg \
    -o /etc/apt/keyrings/docker.asc
$ sudo chmod a+r /etc/apt/keyrings/docker.asc

# Add the Docker repository
$ echo \
  "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.asc] \
  https://download.docker.com/linux/debian \
  $(. /etc/os-release && echo "$VERSION_CODENAME") stable" | \
  sudo tee /etc/apt/sources.list.d/docker.list > /dev/null

# Install Docker Engine
$ sudo apt update
$ sudo apt install -y \
    docker-ce docker-ce-cli containerd.io \
    docker-buildx-plugin docker-compose-plugin

# Add your user to the docker group (no sudo needed for docker)
$ sudo groupadd docker 2>/dev/null || true
$ sudo usermod -aG docker $USER

# Apply group change (or log out and back in)
$ newgrp docker

# Verify Docker installation
$ docker run hello-world
# Should print "Hello from Docker!" if working correctly

# Pull the official ROS 2 Jazzy desktop image (Ubuntu Noble base)
# "desktop" includes RViz2, demo tools, and all CLI utilities
# NOTE: This image is ~2.4 GB — allow 15-30 minutes on typical Wi-Fi
$ docker pull osrf/ros:jazzy-desktop

# Quick test: verify ROS 2 is working inside the container
$ docker run --rm osrf/ros:jazzy-desktop \
    bash -c "source /opt/ros/jazzy/setup.bash && ros2 --version"
# Expected output: ros2 jazzy 0.x.x

# Test talker/listener demo (open TWO terminal tabs for this)
# Terminal 1 — start the talker node:
$ docker run -it --rm --network host osrf/ros:jazzy-desktop \
    bash -c "source /opt/ros/jazzy/setup.bash && \
             ros2 run demo_nodes_cpp talker"

# Terminal 2 — start the listener node:
$ docker run -it --rm --network host osrf/ros:jazzy-desktop \
    bash -c "source /opt/ros/jazzy/setup.bash && \
             ros2 run demo_nodes_py listener"
# You should see: [listener]: I heard: [Hello World: N]

# Create your ROS 2 workspace directory on the host
$ mkdir -p ~/ros2_ws/src

# Create and run the TurboPi container with full hardware access
$ docker run -it \
  --name turbopi_ros2 \
  --restart unless-stopped \
  --network host \
  --privileged \
  -e DISPLAY=$DISPLAY \
  -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
  -v ~/ros2_ws:/root/ros2_ws \
  -v /dev:/dev \
  --device /dev/i2c-1 \
  --device /dev/video0 \
  --device /dev/gpiomem \
  --group-add dialout \
  --group-add video \
  -w /root/ros2_ws \
  osrf/ros:jazzy-desktop \
  bash

# Inside the container — source ROS 2 automatically on every shell
[container]# echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
[container]# echo "source ~/ros2_ws/install/setup.bash 2>/dev/null || true" >> ~/.bashrc
[container]# source ~/.bashrc

# Verify ROS 2 environment inside the container
[container]# ros2 doctor
# All checks should pass

# Start the container after a reboot
$ docker start turbopi_ros2

# Open a shell inside the running container
$ docker exec -it turbopi_ros2 bash

# Open a second shell in the same container (e.g. for teleop)
$ docker exec -it turbopi_ros2 bash

# Stop the container
$ docker stop turbopi_ros2

# View container logs
$ docker logs turbopi_ros2

# Enable X11 forwarding for GUI tools (RViz2, rqt) from host
$ xhost +local:docker

# Install build tools and common ROS 2 packages inside the container
[container]# apt update && apt install -y \
    python3-colcon-common-extensions \
    python3-rosdep \
    python3-vcstool \
    ros-jazzy-teleop-twist-keyboard \
    ros-jazzy-teleop-twist-joy \
    ros-jazzy-joy \
    ros-jazzy-cv-bridge \
    ros-jazzy-image-transport \
    ros-jazzy-camera-info-manager \
    ros-jazzy-rqt-image-view \
    python3-smbus2 \
    python3-opencv \
    python3-numpy \
    python3-pip \
    i2c-tools \
    v4l-utils \
    git

# Initialize rosdep (run once)
[container]# rosdep init 2>/dev/null || true
[container]# rosdep update

# Create workspace structure
[container]# cd ~/ros2_ws
[container]# mkdir -p src

# Build an empty workspace to verify everything works
[container]# colcon build --symlink-install
# Expected: Summary: 0 packages finished [Xs]

# Source the workspace
[container]# source install/setup.bash

# Inside the container, navigate to the workspace src directory
[container]# cd ~/ros2_ws/src

# Clone the turbopi_ros repository
[container]# git clone https://github.com/wltjr/turbopi_ros.git

# Return to workspace root and resolve dependencies
[container]# cd ~/ros2_ws
[container]# rosdep install --from-paths src --ignore-src -r -y

# Install Python dependencies for TurboPi hardware
[container]# pip install smbus2 RPi.GPIO --break-system-packages 2>/dev/null || \
    pip install smbus2 RPi.GPIO

# Build the workspace (--symlink-install allows editing without rebuild)
[container]# colcon build --symlink-install \
    --cmake-args -DCMAKE_BUILD_TYPE=Release
# Build time: ~5-10 minutes on RPi 5 8 GB

# Source the updated workspace
[container]# source install/setup.bash

# Verify packages are available
[container]# ros2 pkg list | grep turbopi
# Expected: turbopi_ros, turbopi_description, turbopi_bringup, etc.

# Verify I2C bus is active on the host
$ ls /dev/i2c*
# Expected: /dev/i2c-1 (and possibly /dev/i2c-2, /dev/i2c-10, etc.)

# Scan I2C bus 1 for connected devices
$ i2cdetect -y 1

# Inside the container: quick I2C connectivity test
[container]# python3 << 'EOF'
import smbus2
import time

# Open I2C bus 1
bus = smbus2.SMBus(1)

# TurboPi expansion board address (check your specific board)
BOARD_ADDR = 0x7f

try:
    # Read a register to verify communication
    data = bus.read_byte(BOARD_ADDR)
    print(f"I2C connection OK — board responded: 0x{data:02x}")
except Exception as e:
    print(f"I2C error: {e}")
    print("Check: sudo usermod -aG i2c $USER && reboot")
finally:
    bus.close()
EOF

# Install libcamera tools and v4l2 utilities
$ sudo apt install -y \
    libcamera-apps \
    v4l-utils \
    v4l2loopback-dkms \
    v4l2loopback-utils \
    python3-libcamera \
    python3-picamera2

# Test camera with libcamera (should show a preview window)
$ libcamera-hello --timeout 5000

# Create a V4L2 virtual device for Docker to access
$ sudo modprobe v4l2loopback \
    devices=1 \
    video_nr=10 \
    card_label="TurboPi Camera" \
    exclusive_caps=1

# Stream libcamera output to the virtual V4L2 device
# Run this in a background terminal or as a systemd service
$ libcamera-vid \
    --width 640 \
    --height 480 \
    --framerate 30 \
    --codec yuv420 \
    --output - | \
    ffmpeg -f rawvideo -pix_fmt yuv420p \
           -s 640x480 -r 30 -i - \
           -f v4l2 /dev/video10 &

# Verify the virtual device is visible
$ v4l2-ctl --list-devices

# Test USB cameras (if using USB webcam instead of CSI)
$ ls /dev/video*
$ v4l2-ctl --device=/dev/video0 --all

# Install V4L2 camera package for ROS 2
[container]# apt install -y ros-jazzy-v4l2-camera

# Launch camera node publishing to /camera/image_raw topic
[container]# ros2 run v4l2_camera v4l2_camera_node \
    --ros-args \
    -p video_device:=/dev/video10 \
    -p image_size:=[640,480] \
    -p camera_frame_id:=camera_link

# In another terminal — view the camera feed
[container]# ros2 run rqt_image_view rqt_image_view
# Or use: ros2 topic echo /camera/image_raw (text only)

# List all topics to verify camera is publishing
[container]# ros2 topic list | grep camera
# Expected: /camera/camera_info  /camera/image_raw

# Check publishing rate (should be ~30 Hz)
[container]# ros2 topic hz /camera/image_raw

# Start the full TurboPi hardware stack (motor controller, servos, sensors)
[container]# ros2 launch turbopi_bringup turbopi_ros.launch.py

# With optional LiDAR (if RPLidar is attached)
[container]# ros2 launch turbopi_bringup turbopi_ros.launch.py lidar:=True

# Open a second terminal in the container
$ docker exec -it turbopi_ros2 bash

# Start keyboard teleop — publishes to /cmd_vel
[container]# ros2 run teleop_twist_keyboard teleop_twist_keyboard


# ── KEYBOARD CONTROLS ──────────────────────────
# i      : forward       ,  : backward
# j      : turn left     l  : turn right
# u      : diagonal FL   o  : diagonal FR
# m      : diagonal BL   .  : diagonal BR
# k      : STOP
# q/z    : increase/decrease speed
# w/x    : increase/decrease only linear speed
# e/c    : increase/decrease only angular speed
# CTRL-C : quit
# ───────────────────────────────────────────────

# Pair DS4 via Bluetooth (on the HOST, before starting container)
$ bluetoothctl
[bluetooth]# power on
[bluetooth]# agent on
[bluetooth]# scan on
# Hold Share + PS button on DS4 until it flashes rapidly
[bluetooth]# pair XX:XX:XX:XX:XX:XX
[bluetooth]# trust XX:XX:XX:XX:XX:XX
[bluetooth]# connect XX:XX:XX:XX:XX:XX
[bluetooth]# exit

# Verify DS4 appears as a joystick device on host
$ ls /dev/input/js*
# Expected: /dev/input/js0

# Inside the container — launch DS4 teleop
[container]# ros2 launch turbopi_teleop teleop_turbopi.launch.py

# DS4 CONTROLS:
# Left Joystick      : Drive (forward/backward/strafe)
# Right Joystick     : Pan-tilt camera control
# L1                 : Slow mode (half speed)
# L2 + R2            : Enable movement (deadman switch)
# Cross (X)          : Stop all motors

# Manual cmd_vel publish for testing (no gamepad needed)
[container]# ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
  "{ linear: { x: 0.2, y: 0.0, z: 0.0 }, \
     angular: { x: 0.0, y: 0.0, z: 0.0 } }"
# Moves forward at 0.2 m/s for one message

# Strafe LEFT (mecanum only) — x=0, y=0.2
[container]# ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
  "{ linear: { x: 0.0, y: 0.2, z: 0.0 }, \
     angular: { x: 0.0, y: 0.0, z: 0.0 } }"

# Navigate to workspace src directory inside the container
[container]# cd ~/ros2_ws/src

# Create a new Python ROS 2 package
[container]# ros2 pkg create my_turbopi_nodes \
    --build-type ament_python \
    --dependencies rclpy std_msgs sensor_msgs geometry_msgs

# Navigate into the package
[container]# cd my_turbopi_nodes/my_turbopi_nodes

#!/usr/bin/env python3
"""
TurboPi Line Follower Node
Reads 4-channel IR sensor array via I2C and
publishes Twist commands to /cmd_vel.
"""
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import smbus2
import time

# TurboPi expansion board I2C address
BOARD_ADDR = 0x7f
# Register for 4-channel IR line sensor
IR_SENSOR_REG = 0x1A

class LineFollowerNode(Node):
    def __init__(self):
        super().__init__('line_follower')

        # Publisher: sends velocity commands to motor controller
        self.cmd_pub = self.create_publisher(
            Twist, '/cmd_vel', 10)

        # I2C connection to TurboPi expansion board
        self.bus = smbus2.SMBus(1)

        # Control loop at 50 Hz
        self.timer = self.create_timer(0.02, self.control_loop)
        self.get_logger().info('Line follower node started')

    def read_ir_sensors(self):
        """Read 4-channel IR sensor state from expansion board.
        Returns list of 4 booleans: True = line detected."""
        try:
            raw = self.bus.read_byte_data(BOARD_ADDR, IR_SENSOR_REG)
            # Each bit represents one sensor channel
            return [(raw >> i) & 1 for i in range(4)]
        except Exception as e:
            self.get_logger().warn(f'IR read error: {e}')
            return [0, 0, 0, 0]

    def control_loop(self):
        sensors = self.read_ir_sensors()
        cmd = Twist()

        # Simple line-following PID logic
        # sensors[0..3] = left to right IR channels
        s = sensors
        if s == [0,1,1,0]:          # Centered on line
            cmd.linear.x  = 0.25
            cmd.angular.z = 0.0
        elif s == [0,0,1,0] or s == [0,0,1,1]:  # Drift left
            cmd.linear.x  = 0.18
            cmd.angular.z = 0.4
        elif s == [0,1,0,0] or s == [1,1,0,0]:  # Drift right
            cmd.linear.x  = 0.18
            cmd.angular.z = -0.4
        elif s == [1,1,1,1]:        # Intersection — go straight
            cmd.linear.x  = 0.2
            cmd.angular.z = 0.0
        else:                        # Line lost — stop
            cmd.linear.x  = 0.0
            cmd.angular.z = 0.0

        self.cmd_pub.publish(cmd)

    def destroy_node(self):
        # Send stop command before shutting down
        self.cmd_pub.publish(Twist())
        self.bus.close()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = LineFollowerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

# Add entry point to setup.py of your package
# In ~/ros2_ws/src/my_turbopi_nodes/setup.py, add to console_scripts:
#   'line_follower = my_turbopi_nodes.line_follower_node:main',

# Build from workspace root
[container]# cd ~/ros2_ws
[container]# colcon build --packages-select my_turbopi_nodes --symlink-install

# Source the updated workspace
[container]# source install/setup.bash

# Run the node (make sure turbopi_bringup is also running)
[container]# ros2 run my_turbopi_nodes line_follower

# Monitor the velocity commands being published
[container]# ros2 topic echo /cmd_vel

# Install OpenCV with DNN support
[container]# apt install -y \
    python3-opencv \
    ros-jazzy-cv-bridge \
    ros-jazzy-image-transport \
    ros-jazzy-image-pipeline

# Install PyTorch for YOLOv5 (ARM64 — use pip wheel)
[container]# pip install torch torchvision torchaudio \
    --index-url https://download.pytorch.org/whl/cpu

# Install Ultralytics YOLOv8 (compatible with YOLO models)
[container]# pip install ultralytics

# Verify OpenCV
[container]# python3 -c "import cv2; print(f'OpenCV {cv2.__version__} OK')"

# Download YOLOv5s model (Hiwonder uses this for TurboPi)
[container]# python3 -c "
from ultralytics import YOLO
model = YOLO('yolov5su.pt')   # Downloads automatically ~28 MB
print('YOLOv5 model loaded OK')
"

#!/usr/bin/env python3
"""
Color Tracking Node for TurboPi
Detects a target color blob in the camera feed,
computes error from image center, and steers the robot
and pan-tilt to follow it.
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32MultiArray
from cv_bridge import CvBridge
import cv2
import numpy as np

# Target color in HSV — RED example
# Tune these values for your specific lighting conditions
COLOR_RANGES = {
    'red':    ([0,  120, 70],  [10, 255, 255]),
    'green':  ([40, 60,  50],  [80, 255, 255]),
    'blue':   ([100,100, 50],  [130,255, 255]),
    'yellow': ([20, 100, 100], [30, 255, 255]),
}

class ColorTrackerNode(Node):
    def __init__(self):
        super().__init__('color_tracker')
        self.declare_parameter('target_color', 'red')
        self.bridge = CvBridge()

        # Subscribe to camera feed
        self.img_sub = self.create_subscription(
            Image, '/camera/image_raw',
            self.image_callback, 10)

        # Publish velocity and servo commands
        self.cmd_pub   = self.create_publisher(Twist, '/cmd_vel', 10)
        self.servo_pub = self.create_publisher(
            Float32MultiArray, '/servo_cmd', 10)

        self.get_logger().info('Color tracker ready')

    def image_callback(self, msg):
        color = self.get_parameter('target_color').value
        if color not in COLOR_RANGES:
            return

        # Convert ROS Image → OpenCV BGR → HSV
        frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
        h, w  = frame.shape[:2]
        hsv   = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        # Create color mask
        lo, hi = [np.array(v) for v in COLOR_RANGES[color]]
        mask   = cv2.inRange(hsv, lo, hi)
        mask   = cv2.dilate(mask, None, iterations=2)

        # Find largest contour (the target object)
        cnts, _ = cv2.findContours(
            mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        cmd = Twist()
        if cnts:
            c   = max(cnts, key=cv2.contourArea)
            area = cv2.contourArea(c)
            if area > 500:
                M  = cv2.moments(c)
                cx = int(M['m10'] / M['m00'])
                cy = int(M['m01'] / M['m00'])

                # Proportional control: error from image center
                err_x = (cx - w // 2) / (w // 2)  # -1 to +1
                err_y = (cy - h // 2) / (h // 2)  # -1 to +1

                # Rotate robot to center object horizontally
                cmd.angular.z = -0.6 * err_x

                # Move forward if object is small, back if large
                size_err = (area - 30000) / 30000
                cmd.linear.x = -0.15 * size_err

                # Pan-tilt servo command [pan_angle, tilt_angle]
                servo_msg = Float32MultiArray()
                servo_msg.data = [
                    90.0 - err_x * 45.0,   # pan servo
                    90.0 - err_y * 30.0    # tilt servo
                ]
                self.servo_pub.publish(servo_msg)

        self.cmd_pub.publish(cmd)

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

# Start full system + color tracker (targeting red objects)
[container]# ros2 launch turbopi_bringup turbopi_ros.launch.py &
[container]# ros2 run my_turbopi_nodes color_tracker \
    --ros-args -p target_color:=red

# Change target color at runtime (no restart needed)
[container]# ros2 param set /color_tracker target_color blue

# View camera with detection overlay using rqt
[container]# ros2 run rqt_image_view rqt_image_view

# Emergency stop — publish zero velocity
[container]# ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{}"

# Move forward 0.3 m/s for 2 seconds
[container]# ros2 topic pub -r 10 --keep-alive 2 /cmd_vel \
    geometry_msgs/msg/Twist "{ linear: { x: 0.3 } }"

# Rotate in place at 0.5 rad/s
[container]# ros2 topic pub -r 10 /cmd_vel \
    geometry_msgs/msg/Twist "{ angular: { z: 0.5 } }"

# Record a rosbag of camera + odometry
[container]# ros2 bag record /camera/image_raw /odom /cmd_vel -o my_run

# Check system health
[container]# ros2 doctor --report

# View TF transforms (robot coordinate frames)
[container]# ros2 run tf2_tools view_frames

# Monitor CPU / temperature on host
$ vcgencmd measure_temp
$ htop