#include "cv_connection.hpp" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include 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(topic_name + std::string("/image_raw") + std::string("/compressed"), 1); // Camera Info //info_manager_.setCameraName(topic_name); if ( info_manager_.validateURL(calib_file_path) ) { info_manager_.loadCameraInfo(calib_file_path); camera_info = info_manager_.getCameraInfo(); } else { // URL not valid, use the old one std::cerr<<"Calibration URL not valid @ "+topic_name+" : "+calib_file_path<(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 &img_msg = *ptr; // >> message to be sent std_msgs::Header header; // empty header header.seq = counter; // user defined counter header.stamp = ros::Time::now(); // time // Formatting directly the message no OpenCV 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; // 1 Byte per 4 Channels of the RGBA format 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 ); //counter ++; } 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); //=COLOR_BGRA2RGB std_msgs::Header header; // empty header header.seq = counter; // user defined counter header.stamp = ros::Time::now(); // time //sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(mat_img).toImageMsg(); 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 ); //counter ++; } 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; // empty header c_img_msg.header = header; //c_img_msg.header.seq = counter; // user defined counter c_img_msg.header.stamp = ros::Time::now(); // time c_img_msg.format = "jpeg"; pub_comp.publish( c_img_msg ); /* camera_info.roi.do_rectify=true; pubCamInfo.publish( camera_info ); */ } /* void OpenCVConnector::WriteToRosPng(unsigned char* buffer, int width, int height) { //sensor_msgs::Image img_msg; // >> message to be sent sensor_msgs::CompressedImage c_img_msg; // std::vector< uint8_t > std_msgs::Header header; // empty header header.seq = counter; // user defined counter header.stamp = ros::Time::now(); // time c_img_msg.header = header; c_img_msg.format = "png"; // Using: lodepng. See references: // https://raw.githubusercontent.com/lvandeve/lodepng/master/examples/example_encode.cpp // http://docs.ros.org/indigo/api/libfovis/html/lodepng_8h_source.html // https://lodev.org/lodepng/ std::vector png; // = c_img_msg.data unsigned bitdepth = 8; LodePNGColorType colortype = LCT_RGBA; unsigned error = lodepng::encode( c_img_msg.data, buffer, width, height, colortype , bitdepth ); if(error) std::cout << "encoder error " << error << ": "<< lodepng_error_text(error) << std::endl; std::cout << " Publishing compressed image"< params = { cv::IMWRITE_JPEG_QUALITY, 95 }; cv::imencode(".jpg", mat_img, c_img_msg.data, params ); std_msgs::Header header; // empty header c_img_msg.header = header; //c_img_msg.header.seq = counter; // user defined counter c_img_msg.header.stamp = ros::Time::now(); // time c_img_msg.format = "jpeg"; pub_comp.publish( c_img_msg ); }