ROS信息的收发

图像信息的收发

图像信息发送

#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;
}