sensor_msgs中常用的传感器数据格式以及c++操作
sensor_msgs中常用的一些传感器有imu、图像、点云、激光雷达点云、里程计信息等等信息。
接下来，我们俩看一下这些传感器数据，以及数据如何操作。
一、pointcloud2
1）随即点云生成
pcl::PointCloud<pcl::PointXYZI> pointxyz;pointxyz.width = 100;pointxyz.height = 1;pointxyz.resize(pointxyz.height * pointxyz.width);srand(time(nullptr));for(int i = 0;i < 100; i ++){pointxyz.points[i].x = (rand()% 1000) / 10.0;pointxyz.points[i].y = (rand()% 1000) / 10.0;pointxyz.points[i].z = (rand()% 1000) / 10.0;}2）点云的发布和接收
//-------------------------------------------点云的生成和转化----------------------------顺利完成---------------------// 这个函数在pcl_conversion./pcl_conversion.h文件中sensor_msgs::PointCloud2 pointCloudMsg;pointCloudMsg.header.stamp = ros::Time().now();pointCloudMsg.header.frame_id = "/camera_init";pcl::toROSMsg(pointxyz,pointCloudMsg);pcl::PointCloud<pcl::PointXYZI> pointxyzi2;pcl::fromROSMsg(pointCloudMsg, pointxyzi2);for( int i = 0; i < pointxyzi2.points.size();i ++ ){std::cout<< pointxyzi2.points[i].x<<" "<< pointxyzi2.points[i].y<<" "<< pointxyzi2.points[i].z<<"\n";}pcl::PointCloud<pcl::PointXYZI> point3;pcl::moveFromROSMsg(pointCloudMsg,point3);for( int i = 0; i < point3.points.size();i ++ ){std::cout<< point3.points[i].x<<" "<< point3.points[i].y<<" "<< point3.points[i].z<<"****\n";}二、laserscan
sensor_msgs::LaserScan laserMsg;laserMsg.angle_min = 0.0; // 开始扫描的角度laserMsg.angle_max = 0.0; //结束扫描的角度laserMsg.angle_increment = 60; // 每一次扫描增加的角度laserMsg.time_increment = 0.1; // 测量的时间间隔laserMsg.scan_time = 0.1; //扫描的时间间隔laserMsg.range_min = 1.0; // 距离的最大值laserMsg.range_max = 40.0; // 距离的最小值laserMsg.ranges.push_back(10); // 激光扫描中存储的一帧数据laserMsg.intensities.push_back(1.0); // 每一个激光点的强度三、image
1）图像的生成和消息发布
ros::Subscriber subImg = nh_.subscribe<sensor_msgs::Image>("/topic_image", 2, &DataProcess::imageCallback, &dp);ros::Publisher pubImg = nh_.advertise<sensor_msgs::Image>("/topic_image", 2);std::string strImgPath = "/home/zhu/pcb_test.png";cv::Mat img = cv::imread(strImgPath,cv::IMREAD_COLOR);std::cout<< img.rows<<" "<< img.cols<<std::endl;//发布消息std_msgs::Header header;header.stamp = ros::Time().now();header.frame_id = "/init";sensor_msgs::ImagePtr imgMsgs = cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8,img).toImageMsg();pubImg.publish(imgMsgs);2）图像的消息接收
void imageCallback(const sensor_msgs::Image::ConstPtr& imgMsg){cv_bridge::CvImagePtr imgPtr ;try{imgPtr = cv_bridge::toCvCopy(imgMsg,"bgr8" );}catch(const cv_bridge::Exception& e){std::cerr << e.what() << '\n';return ;}cv::Mat img = imgPtr->image;double time = imgPtr->header.stamp.toSec();std::string strEncoding = imgPtr->encoding;std::cout<<"time:"<<time << " "<< img.rows<<" "<< img.cols<<std::endl;}四、imu
1）imu数据的生成和发布
sensor_msgs::Imu imuMsg;imuMsg.angular_velocity.x = 0.0;imuMsg.angular_velocity.y = 0;imuMsg.angular_velocity.z = 0;imuMsg.angular_velocity_covariance[0] = 0.0;imuMsg.angular_velocity_covariance.elems[0] = 0.0;imuMsg.header.frame_id = "/camera_init";imuMsg.header.stamp = ros::Time().now();imuMsg.linear_acceleration.x = 0;imuMsg.linear_acceleration.y = 0;imuMsg.linear_acceleration.z = 0;imuMsg.orientation.w = 1.0;imuMsg.orientation.x = 0;imuMsg.orientation.y = 0;imuMsg.orientation.z = 0;ros::Subscriber subImu = nh_.subscribe<sensor_msgs::Imu>("/topic_imu", 200, &DataProcess::imuCallback, &dp);ros::Publisher pubImu = nh_.advertise<sensor_msgs::Imu>("/topic_imu", 200);pubImu.publish(imuMsg);2）imu的接收
void imuCallback(const sensor_msgs::Imu::ConstPtr& imuMsg){std::cout<< imuMsg->linear_acceleration.x<< " "<< imuMsg->linear_acceleration.y<< " "<< imuMsg->linear_acceleration.z<< " \n";}五、nav_msg
1)由于nav_msg并没有需要什么转换的，直接赋值和直接使用就可以了，所以之介绍一下数据类型。
nav_msgs::Odometry odomMsg;odomMsg.header.frame_id = "/camera_init";odomMsg.header.stamp = ros::Time().now();odomMsg.child_frame_id = " child";odomMsg.pose.pose.position.x = 0.0;odomMsg.pose.pose.position.y = 0;odomMsg.pose.pose.position.z = 0;odomMsg.pose.pose.orientation.w = 0.0;odomMsg.pose.pose.orientation.x = 0.0;odomMsg.pose.pose.orientation.y = 0.0;odomMsg.pose.pose.orientation.z = 0.0;odomMsg.twist.twist.angular.x = 0;odomMsg.twist.twist.angular.y = 0;odomMsg.twist.twist.angular.z = 0;odomMsg.twist.twist.linear.x = 0;odomMsg.twist.twist.linear.y = 0;odomMsg.twist.twist.linear.z = 0;最后来看一个合并后的代码
#include<ros/ros.h>
#include<rosbag/bag.h>
#include<rosbag/view.h>
#include<std_msgs/String.h>
#include<pcl/point_types.h>
#include<pcl/point_cloud.h>
#include<pcl/conversions.h>
#include<pcl_conversions/pcl_conversions.h>
#include<opencv2/opencv.hpp>#include<image_transport/image_transport.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/PointCloud2.h>
#include<sensor_msgs/Imu.h>
#include<nav_msgs/Odometry.h>
#include<sensor_msgs/LaserScan.h>class DataProcess
{public:DataProcess(){}public:void processPointCloud2(const sensor_msgs::PointCloud2::ConstPtr& pc){}void imageCallback(const sensor_msgs::Image::ConstPtr& imgMsg){cv_bridge::CvImagePtr imgPtr ;try{imgPtr = cv_bridge::toCvCopy(imgMsg,"bgr8" );}catch(const cv_bridge::Exception& e){std::cerr << e.what() << '\n';return ;}cv::Mat img = imgPtr->image;double time = imgPtr->header.stamp.toSec();std::string strEncoding = imgPtr->encoding;std::cout<<"time:"<<time << " "<< img.rows<<" "<< img.cols<<std::endl;}void imuCallback(const sensor_msgs::Imu::ConstPtr& imuMsg){std::cout<< imuMsg->linear_acceleration.x<< " "<< imuMsg->linear_acceleration.y<< " "<< imuMsg->linear_acceleration.z<< " \n";}};int main(int argc, char** argv)
{ros::init(argc,argv,"rosbaglearn");ros::NodeHandle nh_;DataProcess dp;/*第一、消息的订阅和发布*/ros::Subscriber subPointclod2 = nh_.subscribe<sensor_msgs::PointCloud2>("/pointcloud2",2, &DataProcess::processPointCloud2, &dp);// ros::Publisher pbuLaserCloud = ros::Publisher()ros::Subscriber subPointCLoud2 = nh_.subscribe<sensor_msgs::PointCloud2>("/topic_cloud2", 2, &DataProcess::processPointCloud2, &dp);ros::Publisher pubPointCloud2 = nh_.advertise<sensor_msgs::PointCloud2>("/topic_cloud2", 2);/*点云的转换*/pcl::PointCloud<pcl::PointXYZI> pointxyz;pointxyz.width = 100;pointxyz.height = 1;pointxyz.resize(pointxyz.height * pointxyz.width);srand(time(nullptr));for(int i = 0;i < 100; i ++){pointxyz.points[i].x = (rand()% 1000) / 10.0;pointxyz.points[i].y = (rand()% 1000) / 10.0;pointxyz.points[i].z = (rand()% 1000) / 10.0;}//-------------------------------------------点云的生成和转化----------------------------顺利完成---------------------// 这个函数在pcl_conversion./pcl_conversion.h文件中sensor_msgs::PointCloud2 pointCloudMsg;pointCloudMsg.header.stamp = ros::Time().now();pointCloudMsg.header.frame_id = "/camera_init";pcl::toROSMsg(pointxyz,pointCloudMsg);pcl::PointCloud<pcl::PointXYZI> pointxyzi2;pcl::fromROSMsg(pointCloudMsg, pointxyzi2);for( int i = 0; i < pointxyzi2.points.size();i ++ ){std::cout<< pointxyzi2.points[i].x<<" "<< pointxyzi2.points[i].y<<" "<< pointxyzi2.points[i].z<<"\n";}pcl::PointCloud<pcl::PointXYZI> point3;pcl::moveFromROSMsg(pointCloudMsg,point3);for( int i = 0; i < point3.points.size();i ++ ){std::cout<< point3.points[i].x<<" "<< point3.points[i].y<<" "<< point3.points[i].z<<"****\n";}// std::cout<< int(pointCloudMsg.data) <<" "<< int(&point3.points[0])<<" \n";//--------------------------------------------------------------------------------------------/*---------------------------------------------图像的生成和转化--------------------------------------------------*/ros::Subscriber subImg = nh_.subscribe<sensor_msgs::Image>("/topic_image", 2, &DataProcess::imageCallback, &dp);ros::Publisher pubImg = nh_.advertise<sensor_msgs::Image>("/topic_image", 2);std::string strImgPath = "/home/zhu/pcb_test.png";cv::Mat img = cv::imread(strImgPath,cv::IMREAD_COLOR);std::cout<< img.rows<<" "<< img.cols<<std::endl;//发布消息std_msgs::Header header;header.stamp = ros::Time().now();header.frame_id = "/init";sensor_msgs::ImagePtr imgMsgs = cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8,img).toImageMsg();pubImg.publish(imgMsgs);// 所以图像的中间桥梁就是cv_bridge::CvImagePtr 哈哈哈，这个世界太完美了。/*-----------------------------------------------------------------------------------------------*//*---------------------------------------------------imu消息的收发------start--------------------------------------*/sensor_msgs::Imu imuMsg;imuMsg.angular_velocity.x = 0.0;imuMsg.angular_velocity.y = 0;imuMsg.angular_velocity.z = 0;imuMsg.angular_velocity_covariance[0] = 0.0;imuMsg.angular_velocity_covariance.elems[0] = 0.0;imuMsg.header.frame_id = "/camera_init";imuMsg.header.stamp = ros::Time().now();imuMsg.linear_acceleration.x = 0;imuMsg.linear_acceleration.y = 0;imuMsg.linear_acceleration.z = 0;imuMsg.orientation.w = 1.0;imuMsg.orientation.x = 0;imuMsg.orientation.y = 0;imuMsg.orientation.z = 0;ros::Subscriber subImu = nh_.subscribe<sensor_msgs::Imu>("/topic_imu", 200, &DataProcess::imuCallback, &dp);ros::Publisher pubImu = nh_.advertise<sensor_msgs::Imu>("/topic_imu", 200);pubImu.publish(imuMsg);ros::Rate loop_rate(10);loop_rate.sleep();/*---------------------------------------------------imu消息的收发------end--------------------------------------*/nav_msgs::Odometry odomMsg;odomMsg.header.frame_id = "/camera_init";odomMsg.header.stamp = ros::Time().now();odomMsg.child_frame_id = " child";odomMsg.pose.pose.position.x = 0.0;odomMsg.pose.pose.position.y = 0;odomMsg.pose.pose.position.z = 0;odomMsg.pose.pose.orientation.w = 0.0;odomMsg.pose.pose.orientation.x = 0.0;odomMsg.pose.pose.orientation.y = 0.0;odomMsg.pose.pose.orientation.z = 0.0;odomMsg.twist.twist.angular.x = 0;odomMsg.twist.twist.angular.y = 0;odomMsg.twist.twist.angular.z = 0;odomMsg.twist.twist.linear.x = 0;odomMsg.twist.twist.linear.y = 0;odomMsg.twist.twist.linear.z = 0;sensor_msgs::LaserScan laserMsg;laserMsg.angle_min = 0.0; // 开始扫描的角度laserMsg.angle_max = 0.0; //结束扫描的角度laserMsg.angle_increment = 60; // 每一次扫描增加的角度laserMsg.time_increment = 0.1; // 测量的时间间隔laserMsg.scan_time = 0.1; //扫描的时间间隔laserMsg.range_min = 1.0; // 距离的最大值laserMsg.range_max = 40.0; // 距离的最小值laserMsg.ranges.push_back(10); // 激光扫描中存储的一帧数据laserMsg.intensities.push_back(1.0); // 每一个激光点的强度ros::spin();return 0;
}