`sensor_msgs::PointCloud` 是 ROS 中用于表示点云数据的消息类型。点云是三维空间中的点集合,通常由激光雷达、深度相机或其他传感器生成。`sensor_msgs::PointCloud` 消息用于将点云数据传递给 ROS 节点,以便进行感知、建图和分析等任务。
以下是 `sensor_msgs::PointCloud` 消息的主要字段:
1. `header`:一个 `std_msgs::Header` 类型的消息头,包含时间戳和坐标系信息,用于关联消息的时间和空间信息。
2. `points`:一个 `std::vector<geometry_msgs::Point32>` 类型的容器,其中包含点云中的点。每个 `geometry_msgs::Point32` 包含三个浮点数字段,分别表示点的 x、y 和 z 坐标。
3. `channels`:一个 `std::vector<sensor_msgs::ChannelFloat32>` 类型的容器,用于表示点云的通道信息。通道允许将额外的数据与点相关联,例如颜色、法线、强度等。每个 `sensor_msgs::ChannelFloat32` 包含一个字符串字段 `name`,表示通道的名称,以及一个浮点数数组 `values`,表示通道的值。
`sensor_msgs::PointCloud` 消息通常用于机器人感知和环境建模任务,包括建立三维地图、检测障碍物、进行目标追踪等。通过发布和订阅点云消息,ROS 节点可以获取和处理来自传感器的点云数据。
以下是一个示例 `sensor_msgs::PointCloud` 消息的创建和填充的代码:
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/Point32.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "point_cloud_publisher");
ros::NodeHandle nh;
// 创建一个发布者,发布点云数据到 "point_cloud_data" 主题
ros::Publisher point_cloud_pub = nh.advertise<sensor_msgs::PointCloud>("point_cloud_data", 10);
// 创建一个 PointCloud 消息
sensor_msgs::PointCloud point_cloud_msg;
// 设置消息头部信息
point_cloud_msg.header.stamp = ros::Time::now();
point_cloud_msg.header.frame_id = "base_link";
// 添加点到点云
for (float x = -1.0; x <= 1.0; x += 0.1) {
for (float y = -1.0; y <= 1.0; y += 0.1) {
for (float z = 0.0; z <= 2.0; z += 0.1) {
geometry_msgs::Point32 point;
point.x = x;
point.y = y;
point.z = z;
point_cloud_msg.points.push_back(point);
}
}
}
// 发布点云数据
while (ros::ok()) {
point_cloud_pub.publish(point_cloud_msg);
ros::spinOnce();
ros::Duration(1.0).sleep(); // 1秒发布一次数据
}
return 0;
}
sensor_msgs::PointCloudConstPtr
是 ROS 中用于表示点云消息的常量指针类型。点云消息通常用于表示三维空间中的点集合,通常由激光雷达或深度传感器生成。sensor_msgs::PointCloudConstPtr
表示指向 sensor_msgs::PointCloud
消息的常量指针,用于传递和访问点云数据。
sensor_msgs::PointCloudConstPtr &feature_msg
int feature_id = feature_msg->channels[0].values[i];
int camera_id = feature_msg->channels[1].values[i];
double x = feature_msg->points[i].x;
double y = feature_msg->points[i].y;
double z = feature_msg->points[i].z;
double p_u = feature_msg->channels[2].values[i];
double p_v = feature_msg->channels[3].values[i];
double velocity_x = feature_msg->channels[4].values[i];
double velocity_y = feature_msg->channels[5].values[i];