[PC_HOST]<->[RPI] Transmission du flux vidéo de la caméra

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "cv_bridge/cv_bridge.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/image.hpp"

#include <opencv2/core.hpp>
#include "opencv2/opencv.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui_c.h"
#include "opencv2/videoio/videoio_c.h"

using namespace std::chrono_literals;

class CameraPublisher : public rclcpp::Node
{
public:
  CameraPublisher()
  : Node("RPI_Node"), count_(0)
  {
    // Configuration du profil QoS
 auto qos_profile = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
    qos_profile.reliability(rclcpp::ReliabilityPolicy::BestEffort);
    qos_profile.durability(rclcpp::DurabilityPolicy::Volatile);
    qos_profile.history(rclcpp::HistoryPolicy::KeepLast);
    qos_profile.keep_last(1);

    // Initialisation de la capture vidéo
    if (!capture.open("/dev/video0")) {
      RCLCPP_ERROR(this->get_logger(), "Impossible d'ouvrir la caméra.");
      rclcpp::shutdown();
      return;
    }

    // Création de l'éditeur avec le profil QoS
    publisher_src_frame = this->create_publisher<sensor_msgs::msg::Image>("camera/src_frame", qos_profile);

    capture.set(cv::CAP_PROP_FRAME_WIDTH, 320);
    capture.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
    capture.set(cv::CAP_PROP_FPS, 15);


    // Configuration du timer
    timer_ = this->create_wall_timer(100ms, std::bind(&CameraPublisher::timer_callback, this));
  }

private:
  void timer_callback()
  {

    static int counter = 0;
    counter++;
    if (counter % 5 != 0) return;  // publie 1 frame sur 5


    cv::Mat frame;
    if (!capture.read(frame)) {
      RCLCPP_WARN(this->get_logger(), "Impossible de lire un frame depuis la caméra.");
      return;
    }

    sensor_msgs::msg::Image::SharedPtr msg =
        cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame)
            .toImageMsg();
    publisher_src_frame->publish(*msg.get());
  }

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_src_frame;
  cv::VideoCapture capture;
  size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<CameraPublisher>());
  rclcpp::shutdown();
  return 0;
}

import time
import threading

import rclpy  
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy

import sensor_msgs.msg
from cv_bridge import CvBridge  # To convert to OpenCV image
import cv2


class CameraPublisher(Node):
    def __init__(self):
        super().__init__('rpi_node')

        self.br = CvBridge()
        self.frame_rate = 30  # Max frame rate in Hz
        self.publish_interval = 5  # Publish every 5th frame

        # Configure QoS for the publisher
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )

        self.publisher_src_frame = self.create_publisher(
            sensor_msgs.msg.Image, 'camera/src_frame', qos_profile
        )

        self.i = 0  # Count iterations in the main loop

        # Initialize the video device
        self.cam = cv2.VideoCapture(0)
        if not self.cam.isOpened():
            self.get_logger().error("Failed to open the camera.")
            raise RuntimeError("Camera initialization failed.")
        
        # Set camera properties
        self.cam.set(cv2.CAP_PROP_FRAME_WIDTH, 320)  # Adjust resolution
        self.cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
        # self.cam.set(cv2.CAP_PROP_FPS, 60)  # Adjust FPS if supported by the device

    def loop(self):
        try:
            while rclpy.ok():
                start_time = time.perf_counter()

                self.i += 1

                # Image capture
                success, frame = self.cam.read()
                if not success:
                    self.get_logger().warning("Failed to grab frame. Skipping...")
                    continue

                # Publish every Nth frame
                if self.i % self.publish_interval == 0:
                    # Convert BGR to RGB and publish the image
                    rgb_frame = frame #rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
                    ros_image = self.br.cv2_to_imgmsg(rgb_frame, encoding="rgb8")
                    self.publisher_src_frame.publish(ros_image)

                # Maintain desired frame rate
                elapsed_time = time.perf_counter() - start_time
                sleep_time = max(0, (1 / self.frame_rate) - elapsed_time)
                if sleep_time > 0:
                    time.sleep(sleep_time)

        except Exception as e:
            self.get_logger().error(f"An error occurred: {e}")
        finally:
            # Release camera resource when exiting
            self.cam.release()
            self.get_logger().info("Camera released.")

    def destroy(self):
        """Cleanup resources explicitly."""
        self.cam.release()
        self.destroy_node()


def main(args=None):
    rclpy.init(args=args)

    camera_publisher = CameraPublisher()

    # Starts the loop in a thread because rclpy.spin is blocking
    loop_thread = threading.Thread(target=camera_publisher.loop)
    loop_thread.start()

    try:
        rclpy.spin(camera_publisher)
    except KeyboardInterrupt:
        pass
    finally:
        camera_publisher.destroy()
        rclpy.shutdown()
        loop_thread.join()


if __name__ == '__main__':
    main()

#include "./../include/mainwindow.hpp"

std::shared_ptr<rclcpp::Node> node = nullptr;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher_mode;
rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>::SharedPtr publisher_rgb_low;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscriber_img;

//=========================================================================
MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent)
{
    // Configuration de la fenêtre principale
    this->setMinimumSize(800, 800);
    this->setWindowTitle("ROS2 RECEIVE CAM IHM");

    // Configuration de la zone centrale et de la scène graphique
    QWidget *zoneCentrale = new QWidget;
    setCentralWidget(zoneCentrale);

    graphicsScene = new QGraphicsScene();
    graphicsView = new QGraphicsView();
    QBrush background_scene(QColor(0, 0, 0));
    graphicsScene->setBackgroundBrush(background_scene);
    graphicsView->setScene(graphicsScene);

    m_layout = new QGridLayout();
    m_layout->addWidget(graphicsView, 0, 0);
    zoneCentrale->setLayout(m_layout);

    // Création du nœud ROS2
    node = std::make_shared<rclcpp::Node>("PC_HOST_Node");

    // Configuration d'un profil QoS pour l'abonnement
    auto qos_profile = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
    qos_profile.reliability(rclcpp::ReliabilityPolicy::BestEffort);
    qos_profile.durability(rclcpp::DurabilityPolicy::Volatile);
    qos_profile.history(rclcpp::HistoryPolicy::KeepLast);
    qos_profile.keep_last(1);

    // Création du subscriber
    subscriber_img = node->create_subscription<sensor_msgs::msg::Image>(
        "/camera/src_frame",
        qos_profile,
        std::bind(&MainWindow::img_callback, this, std::placeholders::_1)
    );

    // Configuration du timer pour le spin
    timer_chrono = new QTimer(this);
    connect(timer_chrono, &QTimer::timeout, this, &MainWindow::onTimer_Tick);
    timer_chrono->start(5); // Intervalle de 5 ms

    RCLCPP_INFO(node->get_logger(), "Robot GUI Started");
}

//=========================================================================
MainWindow::~MainWindow()
{
    // Libération des ressources
    RCLCPP_INFO(node->get_logger(), "Robot GUI Shutting Down");
    rclcpp::shutdown();
}

//=========================================================================
void MainWindow::onTimer_Tick()
{
    rclcpp::spin_some(node);
}

//=========================================================================
void MainWindow::img_callback(const sensor_msgs::msg::Image::SharedPtr img)
{
    try
    {
        // Conversion de l'image ROS en QImage (encodage : RGB8)
        QImage image(img->data.data(), img->width, img->height, QImage::Format_BGR888);

        // Affichage de l'image dans la scène graphique
        graphicsScene->clear();
        graphicsScene->addPixmap(QPixmap::fromImage(image));
    }
    catch (const std::exception &e)
    {
        RCLCPP_ERROR(node->get_logger(), "Failed to process image: %s", e.what());
    }
}

#! /usr/bin/python3
#
# colcon build --packages-select pyqt_rcv_cam_ihm_ros2
# . install/setup.bash
# ros2 run pyqt_rcv_cam_ihm_ros2 rcv_cam_ihm

import sys
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Image

from PyQt5 import QtCore
from PyQt5.QtCore import Qt
from PyQt5.QtGui import QColor, QPixmap, QImage
from PyQt5.QtWidgets import (
    QApplication, QMainWindow, QWidget,
    QGraphicsScene, QGraphicsView, QGridLayout
)

########################################################################
# Thread ROS2 
########################################################################
class ROS2Thread(QtCore.QThread):
    image_received = QtCore.pyqtSignal(object)

    def __init__(self):
        super().__init__()
        rclpy.init(args=None)
        self.node = rclpy.create_node('PC_HOST_Node')

        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        # Souscription au topic src_frame
        self.subscription = self.node.create_subscription(
            Image,
            '/camera/src_frame',
            self.callback,
            qos_profile
        )

        self.running = True

    def callback(self, msg):
        # Signal vers le thread principal (IHM)
        self.image_received.emit(msg)

    def run(self):
        while rclpy.ok() and self.running:
            rclpy.spin_once(self.node, timeout_sec=0.01)

    def stop(self):
        self.running = False
        self.node.destroy_node()
        rclpy.shutdown()

########################################################################
# Fenêtre principale PyQt
########################################################################
class MainWindow(QMainWindow):

    def __init__(self, parent=None):
        super(MainWindow, self).__init__(parent)

        self.setMinimumSize(800, 800)
        self.setWindowTitle("ROS2 RECEIVE CAM IHM")

        # Disposition des éléments 
        widget = QWidget()
        self.setCentralWidget(widget)

        self.graphicsScene = QGraphicsScene()
        self.graphicsView = QGraphicsView()
        self.graphicsScene.setBackgroundBrush(QColor(0, 0, 0))
        self.graphicsView.setScene(self.graphicsScene)
        self.graphicsView.setAlignment(Qt.AlignCenter)

        layout = QGridLayout(widget)
        layout.addWidget(self.graphicsView, 0, 0)

        # Lancer ROS2 dans un thread 
        self.ros_thread = ROS2Thread()
        self.ros_thread.image_received.connect(self.img_callback)
        self.ros_thread.start()

        self.pix_item = None

    def closeEvent(self, event):
        if self.ros_thread.isRunning():
            self.ros_thread.stop()
            self.ros_thread.wait()
        event.accept()

    ####################################################################
    def img_callback(self, img):
        try:
            # Conversion ROS2 --> QImage
            image = QImage(img.data, img.width, img.height, QImage.Format_BGR888)

            # MAJ
            pixmap = QPixmap.fromImage(image)
            if self.pix_item is None:
                self.pix_item = self.graphicsScene.addPixmap(pixmap)
            else:
                self.pix_item.setPixmap(pixmap)

        except Exception as e:
            print(f"[Erreur conversion image] {e}")

########################################################################
def main(args=None):
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()
    sys.exit(app.exec_())

########################################################################
if __name__ == '__main__':
    main()
########################################################################