123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176 |
- #include "cv_connection.hpp"
- #include <opencv2/opencv.hpp>
- #include <vector>
- #include <string>
- #include <thread>
- #include <cstdio>
- #include <iostream>
- #include <memory>
- #include <stdexcept>
- #include <array>
- #include <cv_bridge/cv_bridge.h>
- #include <opencv2/imgproc/imgproc.hpp>
- #include <opencv2/opencv.hpp>
- #include <sensor_msgs/Image.h>
- #include <sensor_msgs/image_encodings.h>
- #include <sensor_msgs/CompressedImage.h>
- #include <camera_info_manager/camera_info_manager.h>
- OpenCVConnector::OpenCVConnector(std::string topic_name,size_t csiPort,uint32_t cameraIdx,std::string calib_file_path , std::string camera_type_name, bool rectif_flag ) :
- it(nh), counter(0),
- csiPort(csiPort),
- cameraIdx(cameraIdx),
- info_manager_( ros::NodeHandle( topic_name ) , camera_type_name ),
- do_rectify(rectif_flag) {
-
- pub = it.advertise(topic_name + std::string("/image_raw"), 1);
- pub_comp = nh.advertise<sensor_msgs::CompressedImage>(topic_name + std::string("/image_raw") + std::string("/compressed"), 1);
-
-
-
- if ( info_manager_.validateURL(calib_file_path) ) {
- info_manager_.loadCameraInfo(calib_file_path);
- camera_info = info_manager_.getCameraInfo();
- }
- else {
-
- std::cerr<<"Calibration URL not valid @ "+topic_name+" : "+calib_file_path<<std::endl;
- }
-
- pubCamInfo = nh.advertise<sensor_msgs::CameraInfo>(topic_name + std::string("/camera_info"), 1);
-
- }
- void OpenCVConnector::WriteToOpenCV(unsigned char* buffer, int width, int height) {
- sensor_msgs::ImagePtr ptr = boost::make_shared<sensor_msgs::Image>();
- sensor_msgs::Image &img_msg = *ptr;
-
- std_msgs::Header header;
- header.seq = counter;
- header.stamp = ros::Time::now();
-
-
- img_msg.header = header;
- img_msg.height = height;
- img_msg.width = width;
- img_msg.encoding = sensor_msgs::image_encodings::RGBA8;
-
- img_msg.step = width * 4;
- size_t size = img_msg.step * height;
- img_msg.data.resize(size);
- memcpy((char *)( &img_msg.data[0] ) , buffer , size);
-
- pub.publish( ptr );
-
- camera_info = info_manager_.getCameraInfo();
- camera_info.header = header;
- camera_info.roi.do_rectify = do_rectify;
- pubCamInfo.publish( camera_info );
-
- }
- void OpenCVConnector::WriteToOpenCV_reduced(unsigned char* buffer, int width, int height,int downsample_width , int downsample_height) {
- cv::Mat mat_img(cv::Size(width, height), CV_8UC4, buffer);
- cv::resize(mat_img, mat_img, cv::Size(downsample_width ,downsample_height), 0, 0, CV_INTER_LINEAR);
- cv::cvtColor( mat_img ,mat_img,cv::COLOR_RGBA2RGB);
-
- std_msgs::Header header;
- header.seq = counter;
- header.stamp = ros::Time::now();
-
-
- pub.publish( cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8 , mat_img).toImageMsg() );
-
- camera_info = info_manager_.getCameraInfo();
- camera_info.header = header;
- camera_info.roi.do_rectify = do_rectify;
- pubCamInfo.publish( camera_info );
-
-
- }
- void OpenCVConnector::PublishJpeg(uint8_t* image_compressed, uint32_t image_compressed_size) {
- sensor_msgs::CompressedImage c_img_msg;
-
- c_img_msg.data.resize( image_compressed_size );
- memcpy(&c_img_msg.data[0], image_compressed, image_compressed_size);
-
- std_msgs::Header header;
- c_img_msg.header = header;
-
- c_img_msg.header.stamp = ros::Time::now();
-
- c_img_msg.format = "jpeg";
-
- pub_comp.publish( c_img_msg );
-
-
- }
-
-
- void OpenCVConnector::WriteToOpenCVJpeg(unsigned char* buffer, int width, int height) {
- cv::Mat mat_img(cv::Size(width, height), CV_8UC4 , buffer);
- cv::cvtColor( mat_img ,mat_img,cv::COLOR_BGRA2RGB);
-
- sensor_msgs::CompressedImage c_img_msg;
-
- c_img_msg.data.resize( mat_img.rows*mat_img.cols );
-
- std::vector<int> params = { cv::IMWRITE_JPEG_QUALITY, 95 };
-
- cv::imencode(".jpg", mat_img, c_img_msg.data, params );
-
- std_msgs::Header header;
- c_img_msg.header = header;
-
- c_img_msg.header.stamp = ros::Time::now();
- c_img_msg.format = "jpeg";
-
- pub_comp.publish( c_img_msg );
- }
|