# Config webcam /dev/video0
v4l2-ctl -d /dev/video0 --all
# Affichage image webcam =/dev/video0
mplayer tv:// -tv driver=v4l2:device=/dev/video0:width=640:height=480
# Enregistrement de la video webcam /dev/video0 + son ( input pulseaudio ), enregistrement dans record.mp4
ffmpeg -f pulse -ac 2 -i default -f v4l2 -i /dev/video0 -vcodec libx264 record.mp4
#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)
{
frame= capture.open("/dev/video0");
publisher_src_frame = this->create_publisher<sensor_msgs::msg::Image>("camera/src_frame", 10);
timer_ = this->create_wall_timer(100ms, std::bind(&CameraPublisher::timer_callback, this));
}
private:
void timer_callback()
{
cv_bridge::CvImagePtr cv_ptr;
capture.read(frame);
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;
size_t count_;
cv::VideoCapture capture;
cv::Mat frame ;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<CameraPublisher>());
rclcpp::shutdown();
return 0;
}
unzip cpp_send_camera_ws.zip
cd ./cpp_send_camera_ws
rosdep install -i --from-path src --rosdistro humble -y
colcon build --packages-select cpp_send_camera
. install/setup.bash
ros2 run cpp_send_camera send_camera
import random
import time
import threading
import rclpy # ROS library
from rclpy.node import Node
import std_msgs.msg
import sensor_msgs.msg
from cv_bridge import CvBridge # To convert to open cv image
import cv2
import numpy as np
class CameraPublisher(Node):
def __init__(self):
super().__init__('RPI_Node')
self.br = CvBridge()
self.frame_rate = 50 # Max framerate
# Subscribing to an image topic over the network causes framerate to drop from about 20 to about 10 fps.
self.publisher_src_frame = self.create_publisher(sensor_msgs.msg.Image, 'camera/src_frame', 10) # Raw image
self.lock_color = threading.Lock() # For safe sharing of the color config variables between the main loop and the callback.
self.enable = threading.Condition() # To make the main loop wait passively for the correct mode.
self.lock_mode = threading.Lock() # For safe sharing of the self.mode variable between the main loop and the callback.
self.mode = 0
self.i = 0 # Count interations in the main loop
self.cam = cv2.VideoCapture(0) # Initialise the video device
def loop(self):
while rclpy.ok():
t0 = time.perf_counter()
self.i += 1
# Image capture
succes, frame = self.cam.read()
if not succes:
print("failed to grab frame")
return
if self.i % 10 == 0 :
self.publisher_src_frame.publish(self.br.cv2_to_imgmsg(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB), encoding="rgb8"))
delta_time = time.perf_counter()-t0 # Time to run entire loop
time_sleep = 1/self.frame_rate-delta_time # Time left for chosen framerate
if time_sleep > 0 :
time.sleep(time_sleep)
def main(args=None):
rclpy.init(args=args)
camera_publisher = CameraPublisher()
# Starts the loop in a thread because rclpy.spin is blocking (never comes back) and needs to be called for the callbacks subscriptions to work.
threading.Thread(target = camera_publisher.loop).start()
rclpy.spin(camera_publisher)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
camera_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
unzip py_send_camera_ws.zip
cd ./py_send_camera_ws
rosdep install -i --from-path src --rosdistro humble -y
colcon build --packages-select py_send_camera
. install/setup.bash
ros2 run py_send_camera send_camera
#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)
// ui(new Ui::Form)
{
// ui->setupUi(this);
this->setMinimumSize(800, 800);
this->setWindowTitle("ROS2 RECEIVE CAM IHM");
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);
node = std::make_shared<rclcpp::Node>("PC_HOST_Node");
timer_chrono = new QTimer();
connect( timer_chrono, SIGNAL(timeout()), this, SLOT(onTimer_Tick()));
subscriber_img = node->create_subscription<sensor_msgs::msg::Image>(
"/camera/src_frame", 1000,std::bind(&MainWindow::img_callback, this, std::placeholders::_1));//"/topic/IMAGE_frame"
timer_chrono -> start(5); // 5 ms
RCLCPP_INFO(node->get_logger(), "Robot Gui Started");
}
//=========================================================================
void MainWindow::onTimer_Tick()
{
rclcpp::spin_some(node);
}
MainWindow::~MainWindow()
{
// delete ui;
}
//=========================================================================
void MainWindow::img_callback(const sensor_msgs::msg::Image::SharedPtr img)
{
//Converting the ROS image to a Qimage (encoding: RGB8)
QImage image(img->data.data(), img->width, img->height, QImage::Format_RGB888);
//Display Image in QGraphicsScene
graphicsScene->clear();
graphicsScene->addPixmap(QPixmap::fromImage(image));
}
qt_cpp_rcv_cam_ihm_ros2_ws.zip
#! /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 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')
self.i = 0
self.subscription = self.node.create_subscription(
Image,
'/camera/src_frame',
self.img_callback,
10)
self.subscription # prevent unused variable warning
self.timer = QTimer()
self.timer.timeout.connect(self.onTimerTick)
self.timer.start(5)
def img_callback(self, img):
image = QImage(img.data, img.width, img.height, QImage.Format_RGB888 )
self.graphicsScene.clear();
self.graphicsScene.addPixmap(self.qpixmap.fromImage(image))
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()
########################################################################
unzip pyqt_rcv_cam_ihm_ros2_ws.zip
cd pyqt_rcv_cam_ihm_ros2_ws
rosdep install -i --from-path src --rosdistro humble -y
colcon build --packages-select pyqt_rcv_cam_ihm_ros2
. install/setup.bash
ros2 run pyqt_rcv_cam_ihm_ros2 rcv_cam_ihm