图像信息的收发
图像信息发送
#include <ros/ros.h>
#include <image_transport/image_transport.h> //用于image的订阅和发布 ,并为压缩模式compressed提供支持
#include <opencv2/highgui/highgui.hpp> //应该要设置这里的路径
#include <cv_bridge/cv_bridge.h>//ROS图像消息和OpenCV图像之间进行转换
#include <opencv2/opencv.hpp>
#include <stdio.h>
using namespace cv;
int main(int argc ,char **argv) //argc:在命令行输入参数时,参数的数目//argv:读取所输入的参数的内容(字符串类型)
//如 D:\tc2>test myarg1 myarg2,argc的值是3,argv[0]的值是”test”,argv[1]的值是”myarg1”,argv[2]的值是”myarg2”。
{
ros::init(argc,argv,"pub");//argc 封装参数的个数(n+1);argv 封装参数的数组列表;name 节点名称,有唯一性的限制,禁止包含命名空间
ros::NodeHandle nd;//声明ros句柄nd
image_transport::ImageTransport it(nd);//用之前声明的句柄初始化it
/**
* advertise()函数是你告诉ROS你想在给定的话题名上发布特定类型的消息。
* 在这个advertise()调用之后,master节点将通知任何试图订阅这个话题名称的节点,然后他们将与这个节点建立一个对等网络(peer to peer/P2P)连接。
* advertise()括号里面的第一个参数是话题名字,第二个参数是用于发布消息的消息队列的大小。
* <>里面指定消息的类型
*/
image_transport::Publisher image_publisher=it.advertise("camera/image",1);
// cv::Mat image = cv::imread("//home//wll//ROS_ws//demo_ws//pic_msg_ws//data//malatang.jpg",1);
cv::Mat image = cv::imread("/home/wll/ROS_ws/demo_ws/pic_msg_ws/data/malatang.jpg",1);
if (image.empty())
{
printf("image error!");
}
// cv::imshow("",image);
// cv::waitKey(3000);
// //cv::imshow("",image1);
// //cv::waitKey(3000);
// cv::destroyWindow("");
Mat gray;
cvtColor(image,gray,COLOR_BGR2GRAY);
// //设置提取直方图的相关变量
Mat hist;//用于存放直方图计算结果
const int channels[1]={0};//通道索引
float inRanges[2]={0,65535};
const float*ranges[1]={inRanges};//像素灰度值范围
const int bins[1]={65536};//直方图的维度,其实就是像素灰度值的最大值
calcHist(&image,1,channels,Mat(),hist,1,bins,ranges);//计算图像直方图
//准备绘制直方图
int hist_w=512;
int hist_h=400;
int width=2;
Mat histImage=Mat::zeros(hist_h,hist_w,CV_8UC3);
for(int i=1;i<=hist.rows;++i){
rectangle(histImage,Point(width*(i-1),hist_h-1),
Point(width*i-1,hist_h-cvRound(hist.at<float>(i-1)/20)),
Scalar(255,255,255),-1);
}
namedWindow("histImage",WINDOW_AUTOSIZE);
imshow("histImage",histImage);
imshow("gray",gray);
waitKey(0);
sensor_msgs::ImagePtr image_msg=cv_bridge::CvImage(std_msgs::Header(),"bgr8",image).toImageMsg();
ros::Rate loop_rate(10);//消息发出的频率为10Hz
while(ros::ok())
{
image_publisher.publish(image_msg);
ros::spinOnce();
loop_rate.sleep();
}
}
图像信息接收
#include <ros/ros.h>
#include <image_transport/image_transport.h> //用于image的订阅和发布 ,并为压缩模式compressed提供支持
#include <opencv2/highgui/highgui.hpp> //应该要设置这里的路径
#include <cv_bridge/cv_bridge.h>//ROS图像消息和OpenCV图像之间进行转换
#include <stdio.h>
#include <iostream>
#include <string>
using namespace std;
extern int countt=0;
void img_subscribe(const sensor_msgs::ImageConstPtr& img)
{
cv::Mat img_cv = cv_bridge::toCvShare(img,"mono16")->image;
cv::imshow("imgreceive",img_cv);
cout<<countt<<endl;
countt++;
cv::waitKey(3);
std::string count_str = std::to_string(countt/10);
if(countt|10) cv::imwrite("/home/wll/ROS_ws/demo_ws/pic_msg_ws/data/res/"+count_str+".jpg",img_cv);
}
int main(int argc,char** argv)
{
ros::init(argc,argv,"sub");
ros::NodeHandle nd;
image_transport::ImageTransport img_nd(nd);
// image_transport::Subscriber img_sub = img_nd.subscribe("/optris/image_color",10,&img_subscribe);
image_transport::Subscriber img_sub = img_nd.subscribe("/optris/thermal_image",10,&img_subscribe);
// ros::Rate loop_rate(1);
// while(ros::ok)
// {
// loop_rate.sleep();
// ros::spinOnce();
// }
/**
* ros::spin() 将会进入循环, 一直调用回调函数chatterCallback(),每次调用1000个数据。
* 当用户输入Ctrl+C或者ROS主进程关闭时退出,
*/
ros::spin();//spin()后面不能有除return 0外的其他函数,否则不会调用它
return 0;
}
pointcloud2点云信息的接收和data中数据解析
#include <ros/ros.h> //导入ros
#include <sensor_msgs/PointCloud2.h> //导入消息格式 sensorm_msgs中的点云
#include <pcl_ros/point_cloud.h>
// #include <pcl_ros/point_cloud.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/transforms.h>
#include <geometry_msgs/Pose.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <pcl/common/transforms.h>
#include <pcl/conversions.h>
#include <pcl_ros/impl/transforms.hpp>
#include <iostream>
#include <iomanip>
void pointcloud_sub(const sensor_msgs::PointCloud2::ConstPtr& clouddata) //不转换成pcl,直接使用
{
int pointBytes = clouddata->point_step;
int offset_x;
int offset_y;
int offset_z;
int offset_int;
int offset_ring;
int offset_time;
for (int f=0; f<clouddata->fields.size(); ++f)
{
if (clouddata->fields[f].name == "x")
{
// std::cout << "x" << std::endl;
offset_x=clouddata->fields[f].offset;
}
if (clouddata->fields[f].name == "y")
{
// std::cout << "y" << std::endl;
offset_y=clouddata->fields[f].offset;
}
if (clouddata->fields[f].name == "z")
{
// std::cout << "z" << std::endl;
offset_z=clouddata->fields[f].offset;
}
if (clouddata->fields[f].name == "intensity")
{
// std::cout << "intensity" << std::endl;
offset_int=clouddata->fields[f].offset;
}
if (clouddata->fields[f].name == "ring")
{
// std::cout << "ring" << std::endl;
offset_ring=clouddata->fields[f].offset;
}
if (clouddata->fields[f].name == "timestamp")
{
// std::cout << "timestamp" << std::endl;
offset_time=clouddata->fields[f].offset;
}
// std::cout << "intensity clouddata->fields[f].datatype = " << (int)clouddata->fields[f].datatype << std::endl;
}
// std::cout <<"offset_x:"<< offset_x <<"offset_y:"<< offset_y <<"offset_z:"<< offset_z <<"offset_int"<< offset_int <<"offset_ring:"<<offset_ring<<"offset_timestamp:"<<offset_time<< std::endl;
// std::cout<<"now print every points time :"<<std::endl;
for (int p = 0; p < (clouddata->width * clouddata->height); ++p)
{
// float time = &(clouddata->data[0]) + (pointBytes*p) + offset_time;
// float time = clouddata->data[(pointBytes*p) + offset_time];
// float* pTime = (float*)( &(clouddata->data[0]) + (pointBytes*p) + offset_time);
double* pTime = (double*)( &(clouddata->data[(pointBytes*p) + offset_time]));//C中强制类型转换的方式
// const double* pTime = reinterpret_cast<const double*>( &(clouddata->data[(pointBytes*p) + offset_time]));//C++中强制类型转换的方式
double time = *pTime;
std::cout.precision(22);
std::cout.setf(std::ios::fixed);
std::cout << time << std::endl;
}
}
int main (int argc,char** argv)
{
ros::init(argc,argv,"sub");
ros::NodeHandle nd;
// ros::Subscriber sub = nd.subscribe<sensor_msgs::PointCloud2>("/rslidar_points",1000,pointcloud_sub);
ros::Subscriber sub = nd.subscribe<sensor_msgs::PointCloud2>("/velodyne_points",1000,pointcloud_sub);
//进入自循环,可以尽可能快的调用消息回调函数。
ros::spin();
return 0;
}