#include <rclcpp/rclcpp.hpp> // ROS2 的核心库
#include <sensor_msgs/msg/point_cloud2.hpp> // 点云消息类型
#include <pcl_conversions/pcl_conversions.h> // 用于将 ROS2 点云消息转换为 PCL 格式
#include <pcl/point_types.h> // PCL 点类型
#include <pcl/point_cloud.h> // PCL 点云类型
#include <pcl/common/centroid.h> // 用于计算点云中心
class PointCloudSplitter : public rclcpp::Node
{
public:
PointCloudSplitter() : Node("point_cloud_splitter")
{
// 声明参数，但不提供默认值
//demo👇
this->declare_parameter<double>("z_min",0);
this->declare_parameter<double>("z_max",0);
this->declare_parameter<std::string>("topic_name","OK");
this->declare_parameter<std::string>("filtered_topic_name","OK");
//demo👆
//实际用到的👇
//x+ 相机左侧
//y+ 相机Type-C口的方向
this->declare_parameter<double>("area_1_x", 0.0);
this->declare_parameter<double>("area_1_x_left_slow", 0.0);
this->declare_parameter<double>("area_1_x_right_slow", 0.0);
this->declare_parameter<double>("area_1_y", 0.0);
this->declare_parameter<double>("area_1_z_warning", 0.0);
this->declare_parameter<double>("area_1_z_slow", 0.0);
this->declare_parameter<double>("area_1_z_stop", 0.0);
this->declare_parameter<double>("area_left_x", 0.0);
this->declare_parameter<double>("area_left_y", 0.0);
this->declare_parameter<double>("area_left_z", 0.0);
this->declare_parameter<double>("area_right_x", 0.0);
this->declare_parameter<double>("area_right_y", 0.0);
this->declare_parameter<double>("area_right_z", 0.0);
this->declare_parameter<double>("camer_hight", 0.0);
this->declare_parameter<int>("cut_area_1_x", 0);
this->declare_parameter<int>("cut_area_1_y", 0);
this->declare_parameter<int>("cut_area_1_z", 0);
//实际用到的👆
// 获取参数值
//demo👇
double z_min_ = this->get_parameter("z_min").as_double();
double z_max_ = this->get_parameter("z_max").as_double();
std::string topic_name_ = this->get_parameter("topic_name").as_string();
std::string filtered_topic_name_ = this->get_parameter("filtered_topic_name").as_string();
//demo👆
//实际用到的👇
area_1_x = this->get_parameter("area_1_x").as_double();
area_1_x_left_slow = this->get_parameter("area_1_x_left_slow").as_double();
area_1_x_right_slow = this->get_parameter("area_1_x_right_slow").as_double();
area_1_y = this->get_parameter("area_1_y").as_double();
area_1_z_warning = this->get_parameter("area_1_z_warning").as_double();
area_1_z_slow = this->get_parameter("area_1_z_slow").as_double();
area_1_z_stop = this->get_parameter("area_1_z_stop").as_double();
area_left_x = this->get_parameter("area_left_x").as_double();
area_left_y = this->get_parameter("area_left_y").as_double();
area_left_z = this->get_parameter("area_left_z").as_double();
area_right_x = this->get_parameter("area_right_x").as_double();
area_right_y = this->get_parameter("area_right_y").as_double();
area_right_z = this->get_parameter("area_right_z").as_double();
camer_hight = this->get_parameter("camer_hight").as_double();
cut_area_1_x = this->get_parameter("cut_area_1_x").as_int();
cut_area_1_y = this->get_parameter("cut_area_1_y").as_int();
cut_area_1_z = this->get_parameter("cut_area_1_z").as_int();
//实际用到的👆
// 打印加载的参数
RCLCPP_INFO(this->get_logger(), "Loaded parameters: z_min=%.4f\n", z_min_);
RCLCPP_INFO(this->get_logger(), "z_max=%.4f\n", z_max_);
RCLCPP_INFO(this->get_logger(), "topic_name=%s\n", topic_name_.c_str());
RCLCPP_INFO(this->get_logger(), "filtered_topic_name=%s\n", filtered_topic_name_.c_str());
RCLCPP_INFO(this->get_logger(), "area_1_x=%.4f\n", area_1_x);
RCLCPP_INFO(this->get_logger(), "area_1_x_left_slow=%.4f\n", area_1_x_left_slow);
RCLCPP_INFO(this->get_logger(), "area_1_x_right_slow=%.4f\n", area_1_x_right_slow);
RCLCPP_INFO(this->get_logger(), "area_1_y=%.4f\n", area_1_y);
RCLCPP_INFO(this->get_logger(), "area_1_z_warning=%.4f\n", area_1_z_warning);
RCLCPP_INFO(this->get_logger(), "area_1_z_slow=%.4f\n", area_1_z_slow);
RCLCPP_INFO(this->get_logger(), "area_1_z_stop=%.4f\n", area_1_z_stop);
RCLCPP_INFO(this->get_logger(), "area_left_x=%.4f\n", area_left_x);
RCLCPP_INFO(this->get_logger(), "area_left_y=%.4f\n", area_left_y);
RCLCPP_INFO(this->get_logger(), "area_left_z=%.4f\n", area_left_z);
RCLCPP_INFO(this->get_logger(), "area_right_x=%.4f\n", area_right_x);
RCLCPP_INFO(this->get_logger(), "area_right_y=%.4f\n", area_right_y);
RCLCPP_INFO(this->get_logger(), "area_right_z=%.4f\n", area_right_z);
RCLCPP_INFO(this->get_logger(), "camer_hight=%.4f\n", camer_hight);
RCLCPP_INFO(this->get_logger(), "cut_area_1_x=%d\n", cut_area_1_x);
RCLCPP_INFO(this->get_logger(), "cut_area_1_y=%d\n", cut_area_1_y);
RCLCPP_INFO(this->get_logger(), "cut_area_1_z=%d\n", cut_area_1_z);
// 订阅点云话题
subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/camera/depth/points", 10, std::bind(&PointCloudSplitter::pointCloudCallback, this, std::placeholders::_1));
// 创建一个发布器，用于发布 z 范围在 [0.5, 1) 的点云
filtered_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/filtered_points", 10);
}
private:
double area_1_x;
double area_1_x_left_slow;
double area_1_x_right_slow;
double area_1_y;
double area_1_z_warning;
double area_1_z_slow;
double area_1_z_stop;
double area_left_x;
double area_left_y;
double area_left_z;
double area_right_x;
double area_right_y;
double area_right_z;
double camer_hight;
int cut_area_1_x;
int cut_area_1_y;
int cut_area_1_z;
void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
// 将 ROS2 的 PointCloud2 消息转换为 PCL 点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);
// 初始化计数器
int count_z_0_to_0_5 = 0;
int count_z_0_5_to_1 = 0;
int count_z_1_to_10 = 0;
int count_total = 0;
int count_z_warning_center = 0, count_z_warning_left = 0, count_z_warning_right = 0,
count_z_slow_center = 0, count_z_slow_left = 0, count_z_slow_right = 0,
count_z_stop_center = 0, count_z_stop_left = 0, count_z_stop_right = 0;
bool sign_stop = 0, sign_slow_right = 0, sign_slow_left = 0, sign_slow_center = 0,
sign_warning_center = 0, sign_warning_right = 0, sign_warning_left = 0;
// 创建一个新的点云，用于存储 z 范围在 [0.5, 1) 的点
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 遍历点云，统计不同 z 范围内的点的数量，并提取符合条件的点
for (const auto& point : *cloud)
{
count_total++;
switch (cut_area_1_x) //判断用户对1号区域现在是x方向几划分
{
case 3: //如果是切成了三个区域
if(point.y < camer_hight-0.1 && point.y > -area_1_y-0.1) //排除地面和大于车体高度的点(过滤地面高度的时候，多往上过滤一点，车顶距离往上加一点)
{
filtered_cloud->points.push_back(point); // 将符合条件的点加入新的点云
if(point.z > 0.25 && point.z <= area_1_z_stop) //如果点在停车区内
{
if(point.x <= area_1_x/2 && point.x > (area_1_x/2 - area_1_x_right_slow) )
{
count_z_stop_right++;
}
else if( point.x >= -area_1_x/2 && point.x < (-area_1_x/2 + area_1_x_left_slow) )
{
count_z_stop_left++;
}
else
{
count_z_stop_center++;
}
}
else if(point.z > area_1_z_stop && point.z <= area_1_z_slow) //如果在减速区
{
if(point.x <= area_1_x/2 && point.x > (area_1_x/2 - area_1_x_right_slow) )
{
count_z_slow_right++;
}
else if( point.x >= -area_1_x/2 && point.x < (-area_1_x/2 + area_1_x_left_slow) )
{
count_z_slow_left++;
}
else
{
count_z_slow_center++;
}
}
else if(point.z > area_1_z_slow && point.z <= area_1_z_warning) //如果在警告区
{
if(point.x <= area_1_x/2 && point.x > (area_1_x/2 - area_1_x_right_slow) )
{
count_z_warning_right++;
}
else if( point.x >= -area_1_x/2 && point.x < (-area_1_x/2 + area_1_x_left_slow) )
{
count_z_warning_left++;
}
else
{
count_z_warning_center++;
}
}
}
break;
default: //打印一个错误信息
RCLCPP_INFO(this->get_logger(), "Error！An incorrect value for `cut_area_1_w` was entered. Please check the parameter table for `cut_area_1_w`.");
break;
}
}
//处理各区域的点
//把这些点输出出来，对各个区域定性，进行逻辑判断，得出该停车还是减速
//demo👇
// if (point.z >= 0 && point.z < 0.5)
// {
// if(point.y >= 0 && point.y < 0.5)
// {
// if(point.x >= 0 && point.x < 0.5)
// {
// count_z_0_to_0_5++;
// }
// }
// }
// else if (point.z >= 0.5 && point.z < 1.0)
// {
// count_z_0_5_to_1++;
// filtered_cloud->points.push_back(point); // 将符合条件的点加入新的点云
// }
// else if (point.z >= 1.0 && point.z <= 10.0)
// {
// count_z_1_to_10++;
// }
//demo👆
// RCLCPP_INFO(this->get_logger(), "Safe"); //如果没有点云进入循环输出安全
// 打印统计结果
// RCLCPP_INFO(this->get_logger(), "Points in z range [0, 0.5): %d", count_z_0_to_0_5);
// RCLCPP_INFO(this->get_logger(), "Points in z range [0.5, 1): %d", count_z_0_5_to_1);
// RCLCPP_INFO(this->get_logger(), "Points in z range [1, 10]: %d", count_z_1_to_10);
if(count_z_stop_right > 3500 || count_z_stop_center > 3500 || count_z_stop_left > 3500)
{
sign_stop = 1;
}
if(count_z_slow_right > 3500 )
{
sign_slow_right = 1;
}
if(count_z_slow_left > 3500)
{
sign_slow_left = 1;
}
if(count_z_slow_center > 3500)
{
sign_slow_center = 1;
}
if(count_z_warning_right > 3500)
{
sign_warning_right = 1;
}
if(count_z_warning_left > 3500)
{
sign_warning_left = 1;
}
if(count_z_warning_center > 3500)
{
sign_warning_center = 1;
}
if(sign_stop == 1)
{
RCLCPP_INFO(this->get_logger(), "即将发生碰撞，紧急停车！");
}
else if(sign_slow_right == 1 || sign_slow_left == 1 || sign_slow_center == 1)
{
if(sign_slow_right == 1)
{
RCLCPP_INFO(this->get_logger(), "右侧障碍物过近，减速！");
}
if(sign_slow_center == 1)
{
RCLCPP_INFO(this->get_logger(), "前方障碍物过近，减速！");
}
if(sign_slow_left == 1)
{
RCLCPP_INFO(this->get_logger(), "左侧障碍物过近，减速！");
}
}
else
{
if(sign_warning_right == 1)
{
RCLCPP_INFO(this->get_logger(), "右侧发现障碍物，警告！");
}
if(sign_warning_left == 1)
{
RCLCPP_INFO(this->get_logger(), "前方发现障碍物，警告！");
}
if(sign_warning_center == 1)
{
RCLCPP_INFO(this->get_logger(), "左侧发现障碍物，警告！");
}
}
RCLCPP_INFO(this->get_logger(), "==================");
RCLCPP_INFO(this->get_logger(), "Points in warning_right: %d", count_z_warning_right);
RCLCPP_INFO(this->get_logger(), "Points in warning_left: %d", count_z_warning_left);
RCLCPP_INFO(this->get_logger(), "Points in warning_center: %d", count_z_warning_center);
RCLCPP_INFO(this->get_logger(), "Points in slow_right: %d", count_z_slow_right);
RCLCPP_INFO(this->get_logger(), "Points in slow_left: %d", count_z_slow_left);
RCLCPP_INFO(this->get_logger(), "Points in slow_center: %d", count_z_slow_center);
RCLCPP_INFO(this->get_logger(), "Points in stop_right: %d", count_z_stop_right);
RCLCPP_INFO(this->get_logger(), "Points in stop_left: %d", count_z_stop_left);
RCLCPP_INFO(this->get_logger(), "Points in stop_center: %d", count_z_stop_center);
RCLCPP_INFO(this->get_logger(), "Points in total: %d", count_total);
RCLCPP_INFO(this->get_logger(), "==================");
// 将过滤后的点云转换为 ROS2 消息并发布
if (!filtered_cloud->points.empty())
{
sensor_msgs::msg::PointCloud2 filtered_msg;
pcl::toROSMsg(*filtered_cloud, filtered_msg);
filtered_msg.header = msg->header; // 保持消息头一致
filtered_publisher_->publish(filtered_msg);
}
}
// 订阅器和发布器
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_publisher_;
};
int main(int argc, char** argv)
{
rclcpp::init(argc, argv); // 初始化 ROS2
auto node = std::make_shared<PointCloudSplitter>(); // 创建节点
rclcpp::spin(node); // 运行节点
rclcpp::shutdown(); // 关闭节点
return 0;
}
扫一扫
评论
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
查看更多评论
添加红包
