|
|
@@ -1,16 +1,32 @@
|
|
|
#include "ros/ros.h"
|
|
|
|
|
|
+#include <memory>
|
|
|
+
|
|
|
#include "modulecomm.h"
|
|
|
|
|
|
#include <pcl_conversions/pcl_conversions.h>
|
|
|
#include <pcl_ros/point_cloud.h>
|
|
|
#include <pcl/point_types.h>
|
|
|
|
|
|
+#include <opencv2/opencv.hpp>
|
|
|
+#include <opencv2/core.hpp>
|
|
|
+#include <opencv2/imgproc.hpp>
|
|
|
+#include<cv_bridge/cv_bridge.h>
|
|
|
+#include<sensor_msgs/image_encodings.h>
|
|
|
+#include<image_transport/image_transport.h>
|
|
|
+
|
|
|
+#include "rawpic.pb.h"
|
|
|
+
|
|
|
static std::string _point_topic = "/points_raw";
|
|
|
static std::string _point_msgname = "lidarpc_ros";
|
|
|
|
|
|
+static std::string _image_topic = "/image_raw";
|
|
|
+static std::string _image_msgname = "pic_ros";
|
|
|
+
|
|
|
+
|
|
|
ros::Subscriber points_sub;
|
|
|
void * gpa_lidar;
|
|
|
+ void * gpa_image;
|
|
|
|
|
|
static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
|
|
|
{
|
|
|
@@ -40,6 +56,67 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
|
|
|
}
|
|
|
|
|
|
|
|
|
+int gindex = 0;
|
|
|
+int gcompress = 1;
|
|
|
+void imageCalllback(const sensor_msgs::ImageConstPtr& msg)
|
|
|
+{
|
|
|
+// ROS_INFO("Received \n");
|
|
|
+ try{
|
|
|
+
|
|
|
+ cv::Mat mat1 = cv_bridge::toCvShare(msg, "bgr8")->image ;
|
|
|
+ cv::imshow( "video", mat1 );
|
|
|
+ cv::waitKey(30);
|
|
|
+
|
|
|
+
|
|
|
+ iv::vision::rawpic pic;
|
|
|
+
|
|
|
+ qint64 time = QDateTime::currentMSecsSinceEpoch();
|
|
|
+ QTime xg;
|
|
|
+ xg.start();
|
|
|
+ pic.set_time(time);
|
|
|
+ pic.set_index(gindex);gindex++;
|
|
|
+
|
|
|
+
|
|
|
+ pic.set_elemsize(mat1.elemSize());
|
|
|
+ pic.set_width(mat1.cols);
|
|
|
+ pic.set_height(mat1.rows);
|
|
|
+ pic.set_mattype(mat1.type());
|
|
|
+
|
|
|
+
|
|
|
+ std::vector<int> param = std::vector<int>(2);
|
|
|
+ param[0] = CV_IMWRITE_JPEG_QUALITY;
|
|
|
+ param[1] = 95; // default(95) 0-100
|
|
|
+ std::vector<unsigned char> buff;
|
|
|
+ if(gcompress == 1)
|
|
|
+ {
|
|
|
+ cv::imencode(".jpg", mat1, buff, param);
|
|
|
+ pic.set_picdata(buff.data(),buff.size());
|
|
|
+ buff.clear();
|
|
|
+ pic.set_type(2);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ pic.set_picdata(mat1.data,mat1.rows*mat1.cols*mat1.elemSize());
|
|
|
+ pic.set_type(1);
|
|
|
+ }
|
|
|
+
|
|
|
+ int nSize = pic.ByteSize();
|
|
|
+ std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nSize]);
|
|
|
+ bool bser = pic.SerializeToArray(str_ptr.get(),nSize);
|
|
|
+ if(bser)iv::modulecomm::ModuleSendMsg(gpa_image,str_ptr.get(),nSize);
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout<<"imageCalllback "<< "serialize error. "<<std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ catch( cv_bridge::Exception& e )
|
|
|
+ {
|
|
|
+ ROS_ERROR( "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str() );
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
int main(int argc, char *argv[])
|
|
|
{
|
|
|
ros::init(argc, argv, "rostopilot");
|
|
|
@@ -52,15 +129,26 @@ int main(int argc, char *argv[])
|
|
|
std::cout<<"_point_topic is "<<_point_topic<<std::endl;
|
|
|
std::cout<<"_point_msgname : "<<_point_msgname<<std::endl;
|
|
|
|
|
|
+ private_nh.getParam("image_node",_image_topic);
|
|
|
+ private_nh.getParam("image_msgname",_image_msgname);
|
|
|
+ std::cout<<"_image_topic is "<<_image_topic<<std::endl;
|
|
|
+ std::cout<<"_image_msgname : "<<_image_msgname<<std::endl;
|
|
|
+
|
|
|
gpa_lidar = iv::modulecomm::RegisterSend(_point_msgname.data(),20000000,1);
|
|
|
+ gpa_image = iv::modulecomm::RegisterSend(_image_msgname.data(),20000000,1);
|
|
|
|
|
|
points_sub = private_nh.subscribe(_point_topic, 1, points_callback);
|
|
|
|
|
|
+ image_transport::ImageTransport it(private_nh);
|
|
|
+ image_transport::Subscriber sub = it.subscribe( _image_topic, 1, imageCalllback );
|
|
|
+
|
|
|
|
|
|
+ ros::spin();
|
|
|
+ /*
|
|
|
while (ros::ok())
|
|
|
{
|
|
|
- // ROS_INFO("hello");
|
|
|
ros::spinOnce();
|
|
|
loop_rate.sleep();
|
|
|
}
|
|
|
+ */
|
|
|
}
|
