[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);

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

private:
  void timer_callback()
  {
    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  # ROS library
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, 640)  # Adjust resolution
        # self.cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        # 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 rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy


from std_msgs.msg import String
from sensor_msgs.msg import Image

import sys
from PyQt5 import QtCore
from PyQt5.QtCore import QTimer
from PyQt5.QtGui import QColor, QPixmap, QImage
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QGraphicsScene, QGraphicsView, QGridLayout
 
########################################################################
class MainWindow(QMainWindow):
	
    def __init__(self, parent=None):
        super(MainWindow, self).__init__(parent)

        # Elements de l'IHM
        self.setMinimumSize(800, 800)    
        self.setWindowTitle("ROS2 RECEIVE CAM IHM") 
         
         # Disposition des Elements
        widget = QWidget()
        self.setCentralWidget(widget)

        self.graphicsScene = QGraphicsScene()
        self.graphicsView = QGraphicsView()
        self.graphicsScene.setBackgroundBrush(QColor(0, 0, 0))
        self.graphicsView.setScene(self.graphicsScene)
        self.qpixmap = QPixmap()
        m_layout = QGridLayout(widget);
        m_layout.addWidget(self.graphicsView,0,0)

        # ROS2      
        
        rclpy.init(args=None) 
        self.node = Node('PC_HOST_Node')
        
        
        # Configure QoS profile for publishing and subscribing
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT, 	# Transmission rapide, perte tolérée
            durability=DurabilityPolicy.VOLATILE,		# Pas de stockage d'historique
            history=HistoryPolicy.KEEP_LAST,			# Garder seulement la dernière image
            depth=1										# Profondeur minimale
        )  
 
        self.i = 0
        
        self.subscription = self.node.create_subscription(
            Image,
            '/camera/src_frame',
            self.img_callback,
            qos_profile)
           
        self.subscription  # prevent unused variable warning
        
        self.timer = QTimer()
        self.timer.timeout.connect(self.onTimerTick)   
        self.timer.start(5)      
 
    def img_callback(self, img):
        try:
            # Conversion des données en image
            image = QImage(img.data, img.width, img.height, QImage.Format_BGR888)
            self.graphicsScene.clear()
            self.graphicsScene.addPixmap(QPixmap.fromImage(image))
        except Exception as e:
            self.node.get_logger().error(f"Erreur lors du traitement de l'image : {e}")

    def onTimerTick(self):  
        rclpy.spin_once(self.node)
               
########################################################################
def main(args=None):
 
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()

    sys.exit(app.exec())

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