导航传感器数据只接收激光和点云两种数据 激光数据有标准的消息头,内容为: uint32 seq # seq为每条消息的序列,自动增加 time stamp #stamp消息对应的时间 # fram_id tf参考系ID string frame_id 激光消息的内容: Header header float32 angle_min # start angle of the scan [rad] float32 angle_max # end angle of the scan [rad] float32 angle_increment # angular distance between measurements [rad] float32 time_increment # time between measurements [seconds] float32 scan_time # time between scans [seconds] float32 range_min # minimum range value [m] float32 range_max # maximum range value [m] float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded) float32[] intensities # intensity data [device-specific units] 点云消息内容: Header header geometry_msgs/Point32[] points #Array of 3d points ChannelFloat32[] channels #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point 发布激光数据示例: #include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
int main(int argc, char** argv){
ros::init(argc, argv, "laser_scan_publisher");
ros::NodeHandle n;
ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
unsigned int num_readings = 100;
double laser_frequency = 40;
double ranges[num_readings];
double intensities[num_readings];
int count = 0;
ros::Rate r(1.0);
while(n.ok()){
//generate some fake data for our laser scan
for(unsigned int i = 0; i < num_readings; ++i){
ranges[i] = count;
intensities[i] = 100 + count;
}
ros::Time scan_time = ros::Time::now();
//populate the LaserScan message
sensor_msgs::LaserScan scan;
scan.header.stamp = scan_time;
scan.header.frame_id = "laser_frame";
scan.angle_min = -1.57;
scan.angle_max = 1.57;
scan.angle_increment = 3.14 / num_readings;
scan.time_increment = (1 / laser_frequency) / (num_readings);
scan.range_min = 0.0;
scan.range_max = 100.0;
scan.ranges.resize(num_readings);
scan.intensities.resize(num_readings);
for(unsigned int i = 0; i < num_readings; ++i){
scan.ranges[i] = ranges[i];
scan.intensities[i] = intensities[i];
}
scan_pub.publish(scan);
++count;
r.sleep();
}
} 发布点云数据示例: #include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
int main(int argc, char** argv){
ros::init(argc, argv, "point_cloud_publisher");
ros::NodeHandle n;
ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
unsigned int num_points = 100;
int count = 0;
ros::Rate r(1.0);
while(n.ok()){
sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";
cloud.points.resize(num_points);
//we'll also add an intensity channel to the cloud
cloud.channels.resize(1);
cloud.channels[0].name = "intensities";
cloud.channels[0].values.resize(num_points);
//generate some fake data for our point cloud
for(unsigned int i = 0; i < num_points; ++i){
cloud.points[i].x = 1 + count;
cloud.points[i].y = 2 + count;
cloud.points[i].z = 3 + count;
cloud.channels[0].values[i] = 100 + count;
}
cloud_pub.publish(cloud);
++count;
r.sleep();
}
} |
|