From c44fed89466726448af96724b28eb5df81cb4f4d Mon Sep 17 00:00:00 2001 From: Daniel <1367240116@qq.com> Date: Fri, 13 Dec 2024 15:35:26 +0800 Subject: [PATCH 1/3] add spirecv2-dds cameracppnodes. --- params/spirecv2/default_params.json | 7 + .../ArucoDetCppNodes/aruco_det_demo.cpp | 2 +- spirecv/calib/calib.json | 28 +-- .../dataloader/CameraCppNodes/CMakeLists.txt | 113 +++++++++ .../CameraCppNodes/build_on_jetson.sh | 9 + .../CameraCppNodes/build_on_x86_cuda.sh | 9 + .../CameraCppNodes/build_on_x86_intel.sh | 9 + .../CameraCppNodes/camera_reading_demo.cpp | 17 ++ .../CameraCppNodes/include/sv2_camera_base.h | 86 +++++++ .../CameraCppNodes/include/sv2_camera_input.h | 28 +++ .../CameraCppNodes/include/sv2_camera_read.h | 96 ++++++++ .../CameraCppNodes/src/sv2_camera_base.cpp | 204 +++++++++++++++++ .../CameraCppNodes/src/sv2_camera_input.cpp | 216 ++++++++++++++++++ .../CameraCppNodes/src/sv2_camera_read.cpp | 71 ++++++ 14 files changed, 880 insertions(+), 15 deletions(-) create mode 100644 spirecv/dataloader/CameraCppNodes/CMakeLists.txt create mode 100755 spirecv/dataloader/CameraCppNodes/build_on_jetson.sh create mode 100755 spirecv/dataloader/CameraCppNodes/build_on_x86_cuda.sh create mode 100755 spirecv/dataloader/CameraCppNodes/build_on_x86_intel.sh create mode 100644 spirecv/dataloader/CameraCppNodes/camera_reading_demo.cpp create mode 100644 spirecv/dataloader/CameraCppNodes/include/sv2_camera_base.h create mode 100644 spirecv/dataloader/CameraCppNodes/include/sv2_camera_input.h create mode 100644 spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h create mode 100644 spirecv/dataloader/CameraCppNodes/src/sv2_camera_base.cpp create mode 100644 spirecv/dataloader/CameraCppNodes/src/sv2_camera_input.cpp create mode 100644 spirecv/dataloader/CameraCppNodes/src/sv2_camera_read.cpp diff --git a/params/spirecv2/default_params.json b/params/spirecv2/default_params.json index 3816a0c..3bcdc23 100644 --- a/params/spirecv2/default_params.json +++ b/params/spirecv2/default_params.json @@ -13,12 +13,19 @@ "/VideoFileNode/fps": 1, "/VideoFileNode/repeated": 0, "/VideoFileNode/video_path": "/home/jario/Downloads/THD-Detect-VID.mp4", + "/CameraNode/camera_type": "V4L2CAM", "/CameraNode/camera_id": 0, "/CameraNode/use_v4l2": 1, "/CameraNode/fourcc": "MJPG", + "/CameraNode/camera_ip": "192.168.2.64", "/CameraNode/fps": 30, "/CameraNode/width": 1280, "/CameraNode/height": 720, + "/CameraNode/frame_id": "camera", + "/CameraNode/camera_matrix": [775.08569093, 0.0, 578.64894543, 0.0, 764.40240624, 393.02655515, 0.0, 0.0, 1.0], + "/CameraNode/distortion_coefficients": [0.07637484, -0.09074819, 0.00268185, 0.0007026, 0.03910658], + "/CameraNode/rectification": [0, 0, 0, 0, 0, 0, 0, 0, 0], + "/CameraNode/projection": [775.08569093, 0, 578.64894543, 0, 0, 764.40240624, 393.02655515, 0, 0, 0, 1, 0], "/RTMPStreamerNode/image_sms_url": "/live/sensor/image_raw", "/RTMPStreamerNode/stream_url": "rtmp://monitor-push.byaero.com/boyingLive/BY221423030028", "/RTMPStreamerNode/resize": [ diff --git a/spirecv/algorithm/aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp b/spirecv/algorithm/aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp index e30b696..c1c6514 100644 --- a/spirecv/algorithm/aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp +++ b/spirecv/algorithm/aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp @@ -10,7 +10,7 @@ using namespace std; int main(int argc, char *argv[]) { - sv2::ArucoDetCppNode node("live", "/home/jario/deep/SpireCV/params/spirecv2/default_params.json"); + sv2::ArucoDetCppNode node("live", "/home/amov/SpireCV/params/spirecv2/default_params.json"); node.start(); node.join(); return 0; diff --git a/spirecv/calib/calib.json b/spirecv/calib/calib.json index daa63a4..21c07c7 100644 --- a/spirecv/calib/calib.json +++ b/spirecv/calib/calib.json @@ -1,35 +1,35 @@ { - "/CameraNode/calibration_time": "2024-12-11 13:37:37", + "/CameraNode/calibration_time": "2024-12-11 14:22:21", "/CameraNode/image_width": 640, "/CameraNode/image_height": 480, "/CameraNode/flags": 3, "/CameraNode/flags_description": "use_intrinsic_guess, fix_aspect_ratio", "/CameraNode/camera_matrix": [ - 500.98328427521363, + 703.3114911152534, 0.0, - 327.12863524799246, + 299.1517860552215, 0.0, - 659.8303592497717, - 242.10449640346232, + 935.3364895921571, + 206.95715147197916, 0.0, 0.0, 1.0 ], "/CameraNode/distortion_coefficients": [ - -0.2830680425760443, - 4.922078220437965, - 0.004798810556886978, - -0.0027025001403820433, - -31.492585988061485 + -0.09276871024067203, + 1.105834592345438, + 0.0005653406902169079, + -0.0009866356785947002, + -3.7923478329186953 ], "/CameraNode/projection": [ - 500.98328427521363, + 703.3114911152534, 0.0, - 327.12863524799246, + 299.1517860552215, 0.0, 0.0, - 659.8303592497717, - 242.10449640346232, + 935.3364895921571, + 206.95715147197916, 0.0, 0.0, 0.0, diff --git a/spirecv/dataloader/CameraCppNodes/CMakeLists.txt b/spirecv/dataloader/CameraCppNodes/CMakeLists.txt new file mode 100644 index 0000000..58058f1 --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/CMakeLists.txt @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 3.0 FATAL_ERROR) +cmake_policy(SET CMP0054 NEW) + +set(PROJECT_VERSION 0.2.0) +project(SV2ArucoDet VERSION ${PROJECT_VERSION} LANGUAGES CXX) + +add_definitions(-DAPI_EXPORTS) +set(CMAKE_BUILD_TYPE "Release") + +## JETSON, X86_CUDA, X86_INTEL +message(STATUS "System:${CMAKE_HOST_SYSTEM_PROCESSOR}") +if(NOT DEFINED PLATFORM) + message(FATAL_ERROR "PLATFORM NOT SPECIFIED!") +else() + message(STATUS "PLATFORM: ${PLATFORM}") + if(PLATFORM STREQUAL "JETSON") + add_definitions(-DPLATFORM_JETSON) + option(USE_CUDA "BUILD WITH CUDA." ON) + option(USE_GSTREAMER "BUILD WITH GSTREAMER." ON) + elseif(PLATFORM STREQUAL "X86_CUDA") + add_definitions(-DPLATFORM_X86_CUDA) + option(USE_CUDA "BUILD WITH CUDA." ON) + elseif(PLATFORM STREQUAL "X86_INTEL") + add_definitions(-DPLATFORM_X86_INTEL) + option(USE_INTEL "BUILD WITH INTEL." ON) + option(USE_GSTREAMER "BUILD WITH GSTREAMER." ON) + else() + message(FATAL_ERROR "UNSUPPORTED PLATFORM!") + endif() +endif() + +if(USE_CUDA) + add_definitions(-DWITH_CUDA) + option(CUDA_USE_STATIC_CUDA_RUNTIME OFF) + find_package(CUDA REQUIRED) + message(STATUS "CUDA: ON") +endif() + +if(USE_INTEL) + add_definitions(-DWITH_INTEL) + message(STATUS "INTEL: ON") +endif() + +if(USE_GSTREAMER) + add_definitions(-DWITH_GSTREAMER) + message(STATUS "GSTREAMER: ON") +endif() + +add_definitions(-DWITH_OCV470) +find_package(SpireMS REQUIRED) +find_package(OpenCV 4 REQUIRED) +message(STATUS "OpenCV library status:") +message(STATUS " version: ${OpenCV_VERSION}") +message(STATUS " libraries: ${OpenCV_LIBS}") +message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}") + + +include_directories(${SpireMS_INCLUDE_DIRS}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + + + +if(USE_GSTREAMER) + include_directories(${CMAKE_CURRENT_SOURCE_DIR}/video_io/gstreamer) + if(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "aarch64") + include_directories( + "/usr/include/gstreamer-1.0" + "/usr/local/include/gstreamer-1.0" + "/usr/include/glib-2.0" + "/usr/lib/aarch64-linux-gnu/glib-2.0/include" + ) + elseif(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "x86_64") + include_directories( + "/usr/include/gstreamer-1.0" + "/usr/local/include/gstreamer-1.0" + "/usr/include/glib-2.0" + "/usr/lib/x86_64-linux-gnu/glib-2.0/include" + ) + endif() +endif() + + +# Public header +set(HEADS + include/sv2_camera_base.h + include/sv2_camera_input.h + include/sv2_camera_read.h +) + +set(SRCS + src/sv2_camera_base.cpp + src/sv2_camera_input.cpp + src/sv2_camera_read.cpp +) + + +# add_library(sv2_aruco_det SHARED ${SRCS}) +# target_link_libraries(sv2_aruco_det ${OpenCV_LIBS} ${SpireMS_LIBS}) + +# Demo +add_executable(SV2CameraReadingDemo camera_reading_demo.cpp ${SRCS}) +target_link_libraries(SV2CameraReadingDemo ${OpenCV_LIBS} ${SpireMS_LIBS}) + + + +# install(TARGETS sv2_aruco_det +# LIBRARY DESTINATION lib +# ) +install(TARGETS SV2CameraReadingDemo + RUNTIME DESTINATION bin +) + diff --git a/spirecv/dataloader/CameraCppNodes/build_on_jetson.sh b/spirecv/dataloader/CameraCppNodes/build_on_jetson.sh new file mode 100755 index 0000000..65a5735 --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/build_on_jetson.sh @@ -0,0 +1,9 @@ +#!/bin/bash -e + +rm -rf build +mkdir build +cd build +cmake .. -DPLATFORM=JETSON +make -j4 +sudo make install + diff --git a/spirecv/dataloader/CameraCppNodes/build_on_x86_cuda.sh b/spirecv/dataloader/CameraCppNodes/build_on_x86_cuda.sh new file mode 100755 index 0000000..2b9fd36 --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/build_on_x86_cuda.sh @@ -0,0 +1,9 @@ +#!/bin/bash -e + +rm -rf build +mkdir build +cd build +cmake .. -DPLATFORM=X86_CUDA +make -j4 +sudo make install + diff --git a/spirecv/dataloader/CameraCppNodes/build_on_x86_intel.sh b/spirecv/dataloader/CameraCppNodes/build_on_x86_intel.sh new file mode 100755 index 0000000..d8132eb --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/build_on_x86_intel.sh @@ -0,0 +1,9 @@ +#!/bin/bash -e + +rm -rf build +mkdir build +cd build +cmake .. -DPLATFORM=X86_INTEL +make -j4 +sudo make install + diff --git a/spirecv/dataloader/CameraCppNodes/camera_reading_demo.cpp b/spirecv/dataloader/CameraCppNodes/camera_reading_demo.cpp new file mode 100644 index 0000000..c0197cb --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/camera_reading_demo.cpp @@ -0,0 +1,17 @@ +#include +#include +// 包含SpireMS SDK头文件 +#include +#include "sv2_camera_read.h" + + +using namespace std; + + +int main(int argc, char *argv[]) +{ + sv2::CameraCppNode node("live", "/home/amov/SpireCV/params/spirecv2/default_params.json"); + node.start(); + node.join(); + return 0; +} diff --git a/spirecv/dataloader/CameraCppNodes/include/sv2_camera_base.h b/spirecv/dataloader/CameraCppNodes/include/sv2_camera_base.h new file mode 100644 index 0000000..93fb52c --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/include/sv2_camera_base.h @@ -0,0 +1,86 @@ +#ifndef __SV2_CAMERAIO__ +#define __SV2_CAMERAIO__ + +#include +#include +#include +#include +#include +#include +#include + + + +#define SV_RAD2DEG 57.2957795 +// #define X86_PLATFORM +// #define JETSON_PLATFORM + + +namespace sv2 { + + + +enum class CameraType {NONE, WEBCAM, V4L2CAM, MIPI, RTSP, VIDEO, G1, Q10, GX40, SU17}; + +class CameraBase { +public: + CameraBase(CameraType type=CameraType::NONE, int id=0); + ~CameraBase(); + void open(CameraType type=CameraType::V4L2CAM, int id=0); + bool read(cv::Mat& image); + void release(); + + int getW(); + int getH(); + int getFps(); + std::string getIp(); + int getPort(); + double getBrightness(); + double getContrast(); + double getSaturation(); + double getHue(); + double getExposure(); + std::string getFourcc(); + bool isRunning(); + void setFourcc(std::string fourcc); + void setWH(int width, int height); + void setFps(int fps); + void setIp(std::string ip); + void setRtspUrl(std::string rtsp_url); + void setVideoPath(std::string video_path); + void setPort(int port); + void setBrightness(double brightness); + void setContrast(double contrast); + void setSaturation(double saturation); + void setHue(double hue); + void setExposure(double exposure); +protected: + virtual void openImpl(); + void _run(); + + bool _is_running; + bool _is_updated; + std::thread _tt; + cv::VideoCapture _cap; + cv::Mat _frame; + CameraType _type; + int _camera_id; + + int _width; + int _height; + int _fps; + std::string _ip; + std::string _rtsp_url; + std::string _video_path; + int _port; + double _brightness; + double _contrast; + double _saturation; + double _hue; + double _exposure; + std::string _fourcc; +}; + + +} +#endif diff --git a/spirecv/dataloader/CameraCppNodes/include/sv2_camera_input.h b/spirecv/dataloader/CameraCppNodes/include/sv2_camera_input.h new file mode 100644 index 0000000..9a6741e --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/include/sv2_camera_input.h @@ -0,0 +1,28 @@ +#ifndef __SV2_CAMERA_INPUT__ +#define __SV2_CAMERA_INPUT__ + + +#include +#include +#include +#include +#include +#include "sv2_camera_base.h" + + +namespace sv2 { + + +class Camera : public CameraBase +{ +public: + Camera(); + ~Camera(); +protected: + void openImpl(); + +}; + + +} +#endif diff --git a/spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h b/spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h new file mode 100644 index 0000000..3063867 --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h @@ -0,0 +1,96 @@ +#ifndef __SV2_CAMERA_READ__ +#define __SV2_CAMERA_READ__ + +#include "sms_core.h" +#include "sv2_camera_input.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* +依赖安装 +sudo apt install libopencv-dev +sudo apt install libopencv-contrib-dev + +*/ + +namespace sv2 +{ + + class CameraCppNode : public sms::BaseNode + { + public: + CameraCppNode( + std::string job_name, + std::string param_file, + std::string ip = "127.0.0.1", + int port = 9094) : sms::BaseNode("CameraCppNode", job_name, param_file, ip, port), + _calib_pub("/" + job_name + "/sensor/calibration_info", "sensor_msgs::CameraCalibration"), + _image_pub("/" + job_name + "/sensor/image_raw", "sensor_msgs::CompressedImage") + { + + // 读取节点参数 + this->camera_type = this->get_param("camera_type", "V4L2CAM"); + this->camera_ip = this->get_param("camera_ip", "192.168.2.64"); + this->camera_id = this->get_param("camera_id", 0); + this->image_width = this->get_param("width", 640); + this->image_height = this->get_param("height", 480); + this->fps = this->get_param("fps", 30); + this->frame_id = this->get_param("frame_id", "camera"); + this->camera_matrix = this->get_param("camera_matrix", {7.9379415710551370e+02, 0, 2.9783879354295328e+02, 0, + 7.9491985564466654e+02, 3.0942416136837386e+02, 0, 0, 1}); + this->distortion_coefficients = this->get_param("distortion_coefficients", {2.0950200339181715e-01, -1.1587468096518483e+00, + 5.5342063671841328e-03, 2.2214393775334758e-04, 1.7127431916651392e+00}); + this->rectification = this->get_param("rectification", {0, 0, 0, 0, 0, 0, 0, 0, 0}); + this->projection = this->get_param("projection", {7.9379415710551370e+02, 0, 2.9783879354295328e+02, 0, 0, + 7.9491985564466654e+02, 3.0942416136837386e+02, 0, 0, 0, 1, 0}); + + + logger.info("frame_id: " + this->frame_id); + logger.info("camera_type: " + this->camera_type); + logger.info("image_width: " + std::to_string(static_cast(this->image_width))); + logger.info("image_height: " + std::to_string(static_cast(this->image_height))); + logger.info("fps: " + std::to_string(static_cast(this->fps))); + logger.info("camera_matrix: " + this->camera_matrix.dump()); + logger.info("distortion_coefficients: " + this->distortion_coefficients.dump()); + + // 打开摄像头 + this->cap->setWH(this->image_width, this->image_height); + this->cap->setFps(this->fps); + this->cap->setIp(this->camera_ip); + this->cap->open(this->getCameraType(this->camera_type), this->camera_id); // CameraID 0 + } + ~CameraCppNode() + { + } + void run(); + CameraType getCameraType(const std::string &cameraString); + + cv::Mat img_; + nlohmann::json camera_matrix; + nlohmann::json distortion_coefficients; + nlohmann::json rectification; + nlohmann::json projection; + std::string frame_id; + std::string camera_type; + std::string camera_ip; + int image_width; + int image_height; + int camera_id; + double fps; + Camera *cap = new Camera; + + private: + // 发布话题 + sms::Publisher _image_pub; + sms::Publisher _calib_pub; + }; + +} +#endif diff --git a/spirecv/dataloader/CameraCppNodes/src/sv2_camera_base.cpp b/spirecv/dataloader/CameraCppNodes/src/sv2_camera_base.cpp new file mode 100644 index 0000000..d7bd7d5 --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/src/sv2_camera_base.cpp @@ -0,0 +1,204 @@ +#include "sv2_camera_base.h" + + +#define SV2_MAX_FRAMES 52000 +typedef unsigned char byte; + + +namespace sv2 { + + + +CameraBase::CameraBase(CameraType type, int id) +{ + this->_is_running = false; + this->_is_updated = false; + this->_type = type; + + this->_width = -1; + this->_height = -1; + this->_fps = -1; + this->_ip = "192.168.2.64"; + this->_port = -1; + this->_brightness = -1; + this->_contrast = -1; + this->_saturation = -1; + this->_hue = -1; + this->_exposure = -1; + this->_fourcc = "MJPG"; + + this->open(type, id); +} +CameraBase::~CameraBase() +{ + this->_is_running = false; + // this->_tt.join(); +} +std::string CameraBase::getFourcc() +{ + return this->_fourcc; +} +void CameraBase::setFourcc(std::string fourcc) +{ + this->_fourcc = fourcc; +} +void CameraBase::setWH(int width, int height) +{ + this->_width = width; + this->_height = height; +} +void CameraBase::setFps(int fps) +{ + this->_fps = fps; +} +void CameraBase::setIp(std::string ip) +{ + this->_ip = ip; +} +void CameraBase::setRtspUrl(std::string rtsp_url) +{ + this->_rtsp_url = rtsp_url; +} +void CameraBase::setVideoPath(std::string video_path) +{ + this->_video_path = video_path; +} +void CameraBase::setPort(int port) +{ + this->_port = port; +} +void CameraBase::setBrightness(double brightness) +{ + this->_brightness = brightness; +} +void CameraBase::setContrast(double contrast) +{ + this->_contrast = contrast; +} +void CameraBase::setSaturation(double saturation) +{ + this->_saturation = saturation; +} +void CameraBase::setHue(double hue) +{ + this->_hue = hue; +} +void CameraBase::setExposure(double exposure) +{ + this->_exposure = exposure; +} + +int CameraBase::getW() +{ + return this->_width; +} +int CameraBase::getH() +{ + return this->_height; +} +int CameraBase::getFps() +{ + return this->_fps; +} +std::string CameraBase::getIp() +{ + return this->_ip; +} +int CameraBase::getPort() +{ + return this->_port; +} +double CameraBase::getBrightness() +{ + return this->_brightness; +} +double CameraBase::getContrast() +{ + return this->_contrast; +} +double CameraBase::getSaturation() +{ + return this->_saturation; +} +double CameraBase::getHue() +{ + return this->_hue; +} +double CameraBase::getExposure() +{ + return this->_exposure; +} +bool CameraBase::isRunning() +{ + return this->_is_running; +} + +void CameraBase::openImpl() +{ + +} +void CameraBase::open(CameraType type, int id) +{ + this->release(); + this->_type = type; + this->_camera_id = id; + + openImpl(); + + if (this->_cap.isOpened()) + { + std::cout << "Camera opened!" << std::endl; + this->_is_running = true; + this->_tt = std::thread(&CameraBase::_run, this); + this->_tt.detach(); + } + else if (type != CameraType::NONE) + { + std::cout << "Camera can NOT open!" << std::endl; + } +} +void CameraBase::_run() +{ + while (this->_is_running && this->_cap.isOpened()) + { + if (this->_type != CameraType::VIDEO || this->_is_updated == false) + { + this->_cap >> this->_frame; + this->_is_updated = true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } +} +bool CameraBase::read(cv::Mat& image) +{ + if (this->_type != CameraType::NONE) + { + int n_try = 0; + while (n_try < 5000) + { + if (this->_is_updated) + { + this->_frame.copyTo(image); + this->_is_updated = false; + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + n_try ++; + } + } + if (image.cols == 0 || image.rows == 0) + { + throw std::runtime_error("SpireCV 2.0 (101) Camera cannot OPEN, check CAMERA_ID!"); + } + return image.cols > 0 && image.rows > 0; +} +void CameraBase::release() +{ + this->_is_running = false; + this->_is_updated = false; + if (this->_cap.isOpened()) + this->_cap.release(); +} + +} + diff --git a/spirecv/dataloader/CameraCppNodes/src/sv2_camera_input.cpp b/spirecv/dataloader/CameraCppNodes/src/sv2_camera_input.cpp new file mode 100644 index 0000000..22a62fe --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/src/sv2_camera_input.cpp @@ -0,0 +1,216 @@ +#include "sv2_camera_input.h" +#include +#include + +namespace sv2 { + +Camera::Camera() +{ +} + +Camera::~Camera() +{ +} + +void Camera::openImpl() +{ + if (this->_type == CameraType::WEBCAM || this->_type == CameraType::V4L2CAM) + { + if (this->_type == CameraType::V4L2CAM) + { + this->_cap.open(this->_camera_id, cv::CAP_V4L2); + } + if (this->_type == CameraType::WEBCAM) + { + this->_cap.open(this->_camera_id); + } + if (_fourcc.size() >= 4) + { + const char *fourcc_cstr = _fourcc.c_str(); + this->_cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc(fourcc_cstr[0], fourcc_cstr[1], fourcc_cstr[2], fourcc_cstr[3])); + } + if (this->_width > 0 && this->_height > 0) + { + this->_cap.set(cv::CAP_PROP_FRAME_WIDTH, this->_width); + this->_cap.set(cv::CAP_PROP_FRAME_HEIGHT, this->_height); + } + if (this->_fps > 0) + { + this->_cap.set(cv::CAP_PROP_FPS, this->_fps); + } + if (this->_brightness > 0) + { + this->_cap.set(cv::CAP_PROP_BRIGHTNESS, this->_brightness); + } + if (this->_contrast > 0) + { + this->_cap.set(cv::CAP_PROP_CONTRAST, this->_contrast); + } + if (this->_saturation > 0) + { + this->_cap.set(cv::CAP_PROP_SATURATION, this->_saturation); + } + if (this->_hue > 0) + { + this->_cap.set(cv::CAP_PROP_HUE, this->_hue); + } + if (this->_exposure > 0) + { + this->_cap.set(cv::CAP_PROP_EXPOSURE, this->_exposure); + } + } + else if (this->_type == CameraType::G1) + { + char pipe[512]; + if (this->_width <= 0 || this->_height <= 0) + { + this->_width = 1280; + this->_height = 720; + } + if (this->_port <= 0) + { + this->_port = 554; + } + if (this->_fps <= 0) + { + this->_fps = 30; + } + +#if defined(PLATFORM_X86_CUDA) || defined(PLATFORM_X86_INTEL) + sprintf(pipe, "rtspsrc location=rtsp://%s:%d/H264?W=%d&H=%d&FPS=%d&BR=4000000 latency=100 ! \ + application/x-rtp,media=video ! rtph264depay ! parsebin ! avdec_h264 ! \ + videoconvert ! appsink sync=false", + this->_ip.c_str(), this->_port, this->_width, this->_height, this->_fps); +#endif +#ifdef PLATFORM_JETSON + sprintf(pipe, "rtspsrc location=rtsp://%s:%d/H264?W=%d&H=%d&FPS=%d&BR=4000000 latency=100 ! \ + application/x-rtp,media=video ! rtph264depay ! parsebin ! nvv4l2decoder enable-max-performancegst=1 ! \ + nvvidconv ! video/x-raw,format=(string)BGRx ! videoconvert ! appsink sync=false", + this->_ip.c_str(), this->_port, this->_width, this->_height, this->_fps); +#endif + + // printf("%s\r\n",pipe); + this->_cap.open(pipe, cv::CAP_GSTREAMER); + } + else if (this->_type == CameraType::GX40) + { + std::ostringstream camera_url; + if (this->_width <= 0 || this->_height <= 0) + { + this->_width = 1280; + this->_height = 720; + } + if (this->_port <= 0) + { + this->_port = 554; + } +#if defined(PLATFORM_X86_CUDA) || defined(PLATFORM_X86_INTEL) + camera_url << "rtspsrc location = rtsp://user:0000@" << this->_ip << ":" << this->_port + << "/cam/realmonitor?channel=1&subtype=0 latency=100 ! application/x-rtp,media=video ! \ + rtph265depay ! parsebin ! avdec_h265 ! videoscale ! video/x-raw,width=(int)" << this->_width + << ",height=(int)" << this->_height << " ! videoflip video-direction=4 ! videoconvert ! video/x-raw,format=(string)BGR ! \ + appsink sync=false"; + this->_cap.open(camera_url.str(), cv::CAP_GSTREAMER); +#endif +#ifdef PLATFORM_JETSON + camera_url << "rtspsrc location = rtsp://user:0000@" << this->_ip << ":" << this->_port + << "/cam/realmonitor?channel=1&subtype=0 latency=100 ! \ + application/x-rtp,media=video ! rtph265depay ! parsebin ! \ + nvv4l2decoder ! nvvidconv ! \ + video/x-raw,format=(string)BGRx,width=(int)" + << this->_width << ",height=(int)" << this->_height << " ! videoconvert ! video/x-raw,format=(string)BGR ! \ + appsink sync=false"; + this->_cap.open(camera_url.str(), cv::CAP_GSTREAMER); +#endif + } + else if (this->_type == CameraType::SU17) + { + char pipe[512]; + if (this->_width <= 0 || this->_height <= 0) + { + this->_width = 1280; + this->_height = 720; + } + if (this->_port <= 0) + { + this->_port = 1234; + } + if (this->_fps <= 0) + { + this->_fps = 25; + } + +#if defined(PLATFORM_X86_INTEL) + sprintf(pipe, "rtspsrc location=rtsp://%s:1234/test.sdp latency=100 ! \ + application/x-rtp,media=video ! rtph264depay ! parsebin ! vaapih264dec ! \ + vaapipostproc ! video/x-raw,format=YV12, width=(int)%d, height=(int)%d,framerate=25/1 ! videoconvert ! videorate ! video/x-raw,framerate=25/1 ! \ + appsink sync=false", + this->_ip.c_str(), this->_width, this->_height, this->_fps); + this->_cap.open(pipe, cv::CAP_GSTREAMER); +#endif + } + else if (this->_type == CameraType::MIPI) + { + char pipe[512]; + if (this->_width <= 0 || this->_height <= 0) + { + this->_width = 1280; + this->_height = 720; + } + if (this->_fps <= 0) + { + this->_fps = 30; + } + + sprintf(pipe, "nvarguscamerasrc sensor-id=%d ee-mode=0 tnr-mode=0 aeantibanding=0 wbmode=0 ! \ + video/x-raw(memory:NVMM), width=(int)%d, height=(int)%d, format=(string)NV12, framerate=(fraction)%d/1 ! \ + nvvidconv ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink", + this->_camera_id, this->_width, this->_height, this->_fps); + this->_cap.open(pipe, cv::CAP_GSTREAMER); + } + else if (this->_type == CameraType::RTSP) + { + char pipe[512]; + if (this->_width <= 0 || this->_height <= 0) + { + this->_width = 1280; + this->_height = 720; + } + if (this->_port <= 0) + { + this->_port = 554; + } + if (this->_fps <= 0) + { + this->_fps = 30; + } + +#if defined(PLATFORM_X86_CUDA) + sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! \ + application/x-rtp,media=video ! rtph264depay ! parsebin ! avdec_h264 ! \ + videoconvert ! appsink sync=false", + this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps); + this->_cap.open(pipe, cv::CAP_GSTREAMER); +#endif +#if defined(PLATFORM_X86_INTEL) + sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! \ + application/x-rtp,media=video ! rtph264depay ! parsebin ! vaapih264dec ! \ + vaapipostproc ! video/x-raw,format=YV12,framerate=25/1 ! videoconvert ! videorate ! video/x-raw,framerate=25/1 ! \ + appsink sync=false", + this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps); + this->_cap.open(pipe, cv::CAP_GSTREAMER); +#endif + + +#ifdef PLATFORM_JETSON + sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! application/x-rtp,media=video ! rtph264depay ! parsebin ! nvv4l2decoder enable-max-performancegst=1 ! nvvidconv ! video/x-raw,format=(string)BGRx ! videoconvert ! appsink sync=false", this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps); + this->_cap.open(pipe, cv::CAP_GSTREAMER); +#endif + } + else if (this->_type == CameraType::VIDEO) + { + this->_cap.open(this->_video_path); + } +} + +} diff --git a/spirecv/dataloader/CameraCppNodes/src/sv2_camera_read.cpp b/spirecv/dataloader/CameraCppNodes/src/sv2_camera_read.cpp new file mode 100644 index 0000000..00211ec --- /dev/null +++ b/spirecv/dataloader/CameraCppNodes/src/sv2_camera_read.cpp @@ -0,0 +1,71 @@ +#include "sv2_camera_read.h" +#include +#include + + + +namespace sv2 { + +using namespace cv; + +CameraType CameraCppNode::getCameraType(const std::string &cameraString) +{ + std::unordered_map cameraTypeMap = { + {"NONE", CameraType::NONE}, + {"WEBCAM", CameraType::WEBCAM}, + {"V4L2CAM", CameraType::V4L2CAM}, + {"MIPI", CameraType::MIPI}, + {"RTSP", CameraType::RTSP}, + {"VIDEO", CameraType::VIDEO}, + {"G1", CameraType::G1}, + {"Q10", CameraType::Q10}, + {"GX40", CameraType::GX40}, + {"SU17", CameraType::SU17} + }; + auto it = cameraTypeMap.find(cameraString); + if (it != cameraTypeMap.end()) + { + return it->second; + } + return CameraType::NONE; +} + +void CameraCppNode::run() +{ + auto start_time = std::chrono::steady_clock::now(); + while (this->is_running()) + { + this->cap->read(img_); + nlohmann::json img_msg = sms::cvimg2sms(img_); + + auto current = std::chrono::steady_clock::now(); + std::chrono::duration elapsed_seconds = current - start_time; + if (elapsed_seconds.count() >= 1.0) { + + + nlohmann::json calib_msg = sms::def_msg("sensor_msgs::CameraCalibration"); + calib_msg["type"] = "sensor_msgs::CameraCalibration"; + calib_msg["frame_id"] = this->frame_id; + calib_msg["width"] = this->image_width; + calib_msg["height"] = this->image_height; + calib_msg["distortion_model"]= "plumb_bob"; + calib_msg["D"] = this->camera_matrix; + calib_msg["K"] = this->distortion_coefficients; + calib_msg["R"] = this->rectification; + calib_msg["P"] = this->projection; + + // 发送sensor_msgs::CameraCalibration话题 + _calib_pub.publish(calib_msg); + + start_time = current; + } + + // 发送sensor_msgs::CompressedImage话题 + _image_pub.publish(img_msg); + + // cv::imshow("image", image); + // cv::waitKey(5); + } +} + +} -- Gitee From 407a145fee6f0b16cac098da7c2815b5f65110d0 Mon Sep 17 00:00:00 2001 From: Daniel <1367240116@qq.com> Date: Fri, 13 Dec 2024 17:09:06 +0800 Subject: [PATCH 2/3] fix spirecv2-dds cameranode's name. --- spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h b/spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h index 3063867..ae2dd36 100644 --- a/spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h +++ b/spirecv/dataloader/CameraCppNodes/include/sv2_camera_read.h @@ -30,7 +30,7 @@ namespace sv2 std::string job_name, std::string param_file, std::string ip = "127.0.0.1", - int port = 9094) : sms::BaseNode("CameraCppNode", job_name, param_file, ip, port), + int port = 9094) : sms::BaseNode("CameraNode", job_name, param_file, ip, port), _calib_pub("/" + job_name + "/sensor/calibration_info", "sensor_msgs::CameraCalibration"), _image_pub("/" + job_name + "/sensor/image_raw", "sensor_msgs::CompressedImage") { -- Gitee From b64c8059a0f027181309409e21eb63ecf344f1e2 Mon Sep 17 00:00:00 2001 From: Daniel <1367240116@qq.com> Date: Sat, 14 Dec 2024 14:44:19 +0800 Subject: [PATCH 3/3] add CameraCppNodes for spirecv2-dds. --- ...Node_params.json => cameracppnode_params.json} | 2 +- params/spirecv2/default_params.json | 7 ------- .../aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp | 15 ++++++++++++++- .../CameraCppNodes/camera_reading_demo.cpp | 15 ++++++++++++++- 4 files changed, 29 insertions(+), 10 deletions(-) rename params/spirecv2/{CameraCppNode_params.json => cameracppnode_params.json} (93%) diff --git a/params/spirecv2/CameraCppNode_params.json b/params/spirecv2/cameracppnode_params.json similarity index 93% rename from params/spirecv2/CameraCppNode_params.json rename to params/spirecv2/cameracppnode_params.json index 062dcf7..373173e 100644 --- a/params/spirecv2/CameraCppNode_params.json +++ b/params/spirecv2/cameracppnode_params.json @@ -11,6 +11,6 @@ "/CameraCppNode/camera_matrix": [775.08569093, 0.0, 578.64894543, 0.0, 764.40240624, 393.02655515, 0.0, 0.0, 1.0], "/CameraCppNode/distortion_coefficients": [0.07637484, -0.09074819, 0.00268185, 0.0007026, 0.03910658], "/CameraCppNode/rectification": [0, 0, 0, 0, 0, 0, 0, 0, 0], - "/CameraCppNode/projection": [775.08569093, 0, 578.64894543, 0, 0, 764.40240624, 393.02655515, 0, 0, 0, 1, 0], + "/CameraCppNode/projection": [775.08569093, 0, 578.64894543, 0, 0, 764.40240624, 393.02655515, 0, 0, 0, 1, 0] } diff --git a/params/spirecv2/default_params.json b/params/spirecv2/default_params.json index 3bcdc23..3816a0c 100644 --- a/params/spirecv2/default_params.json +++ b/params/spirecv2/default_params.json @@ -13,19 +13,12 @@ "/VideoFileNode/fps": 1, "/VideoFileNode/repeated": 0, "/VideoFileNode/video_path": "/home/jario/Downloads/THD-Detect-VID.mp4", - "/CameraNode/camera_type": "V4L2CAM", "/CameraNode/camera_id": 0, "/CameraNode/use_v4l2": 1, "/CameraNode/fourcc": "MJPG", - "/CameraNode/camera_ip": "192.168.2.64", "/CameraNode/fps": 30, "/CameraNode/width": 1280, "/CameraNode/height": 720, - "/CameraNode/frame_id": "camera", - "/CameraNode/camera_matrix": [775.08569093, 0.0, 578.64894543, 0.0, 764.40240624, 393.02655515, 0.0, 0.0, 1.0], - "/CameraNode/distortion_coefficients": [0.07637484, -0.09074819, 0.00268185, 0.0007026, 0.03910658], - "/CameraNode/rectification": [0, 0, 0, 0, 0, 0, 0, 0, 0], - "/CameraNode/projection": [775.08569093, 0, 578.64894543, 0, 0, 764.40240624, 393.02655515, 0, 0, 0, 1, 0], "/RTMPStreamerNode/image_sms_url": "/live/sensor/image_raw", "/RTMPStreamerNode/stream_url": "rtmp://monitor-push.byaero.com/boyingLive/BY221423030028", "/RTMPStreamerNode/resize": [ diff --git a/spirecv/algorithm/aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp b/spirecv/algorithm/aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp index e30b696..d868068 100644 --- a/spirecv/algorithm/aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp +++ b/spirecv/algorithm/aruco_det/ArucoDetCppNodes/aruco_det_demo.cpp @@ -10,7 +10,20 @@ using namespace std; int main(int argc, char *argv[]) { - sv2::ArucoDetCppNode node("live", "/home/jario/deep/SpireCV/params/spirecv2/default_params.json"); + std::string job_name = "live"; + std::string config = ""; + + if (argc < 2) + { + std::cout << "Please input SpireCV Config." << std::endl; + } + config = argv[1]; + if (argc > 2) + { + job_name = argv[2]; + } + + sv2::ArucoDetCppNode node(job_name, config); node.start(); node.join(); return 0; diff --git a/spirecv/dataloader/CameraCppNodes/camera_reading_demo.cpp b/spirecv/dataloader/CameraCppNodes/camera_reading_demo.cpp index c0197cb..3cc1ab8 100644 --- a/spirecv/dataloader/CameraCppNodes/camera_reading_demo.cpp +++ b/spirecv/dataloader/CameraCppNodes/camera_reading_demo.cpp @@ -10,7 +10,20 @@ using namespace std; int main(int argc, char *argv[]) { - sv2::CameraCppNode node("live", "/home/amov/SpireCV/params/spirecv2/default_params.json"); + std::string job_name = "live"; + std::string config = ""; + + if (argc < 2) + { + std::cout << "Please input SpireCV Config." << std::endl; + } + config = argv[1]; + if (argc > 2) + { + job_name = argv[2]; + } + + sv2::CameraCppNode node(job_name, config); node.start(); node.join(); return 0; -- Gitee