#include "FenceController.h"


ros::Publisher g_cluster_publisher[4];
ros::Publisher g_src_publisher;

int main(int argc, char** argv)
{
//    EleFence::globalParam param;
//    if(ReadProtoParam("/home/zx/zzw/catkin_ws/src/FindWheel/scripts/setting.prototxt",param)==false)
//    {
//        ROS_ERROR("load proto param failed...");
//        return -1;
//    }

    ros::init(argc, argv, "find_wheel");
    //FenceController fc("/home/zx/zzw/catkin_ws/src/FindWheel/scripts/setting.prototxt");
    FenceController fc("/home/youchen/Documents/measure/MainStructure/elecfence_ws/src/findwheel/scripts/setting.prototxt");
    if(fc.m_initialized) {
//
//        ::ros::NodeHandle g_ros_handle;
//        int LidarCount = param.lidars_size();
//        std::vector<Lidar *> pLidars;
//        pLidars.resize(LidarCount);
//        for (int i = 0; i < param.lidars_size(); ++i) {
//
//        }
//
//        for (int i = 0; i < LIDAR_COUNT; ++i) {
//            g_clouds[i] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
//
//        }
//        for (int i = 0; i < 4; ++i) {
//            char buf[255] = {0};
//            sprintf(buf, "/cloud_wheel_%d", i + 1);
//            g_cluster_publisher[i] = g_ros_handle.advertise<sensor_msgs::PointCloud2>(buf, 1);
//        }
//
//        for (int i = 0; i < LIDAR_COUNT; ++i) {
//            char topic[255] = {0};
//            sprintf(topic, "/scan%d", i + 1);
//            g_cloud_sub[i] =
//                g_ros_handle.subscribe<sensor_msgs::LaserScan>(topic, 1, boost::bind(cloud_callback, _1, i));
//
//        }
        g_src_publisher = fc.m_handle.advertise<sensor_msgs::PointCloud2>("cloud_in", 1);

/////过滤xy


        ros::Rate loop_rate(10);
        pcl::PointCloud<pcl::PointXYZ>::Ptr merged_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
        while (::ros::ok()) {
            //g_src_publisher.publish(fc.p_merged_cloud);
            fc.GetCloud(merged_cloud);
            if (merged_cloud->size() > 0) {
                sensor_msgs::PointCloud2 cloudglobalMsg;
                pcl::toROSMsg(*merged_cloud, cloudglobalMsg);
                cloudglobalMsg.header.stamp = ros::Time::now();
                cloudglobalMsg.header.frame_id = "/map";
                g_src_publisher.publish(cloudglobalMsg);
            }

//            for (int i = 0; i < fc.m_region_results.size(); ++i) {
//                std::cout<<fc.m_region_results[i]? "True" : "False";
//            }
//            std::cout<<std::endl;
//            pcl::PointCloud<pcl::PointXYZ> cloud;
//            for (int i = 0; i < LIDAR_COUNT; ++i)
//                cloud += (*g_clouds[i]);
//
//            if (cloud.size() > 0) {
//                float minx = g_param.regions(0).minx();
//                float maxx = g_param.regions(0).maxx();
//                float miny = g_param.regions(0).miny();
//                float maxy = g_param.regions(0).maxy();
//
//                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//
//                pcl::PassThrough<pcl::PointXYZ> pass;
//                pass.setInputCloud(cloud.makeShared());
//                pass.setFilterFieldName("x");//设置想在哪个坐标轴上操作
//                pass.setFilterLimits(minx, maxx);//将x轴的0到1范围内
//                pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
//                pass.filter(*cloud_filtered);//输出到结果指针
//
//                pass.setInputCloud(cloud_filtered);
//                pass.setFilterFieldName("y");//设置想在哪个坐标轴上操作
//                pass.setFilterLimits(miny, maxy);//将x轴的0到1范围内
//                pass.setFilterLimitsNegative(false);//保留（true就是删除，false就是保留而删除此区间外的）
//                pass.filter(*cloud_filtered);//输出到结果指针
//
//                if (wheel.detect(cloud_filtered)) {
//                    //发布聚类结果图
//                    for (int i = 0; i < 4; ++i) {
//                        if (wheel.m_clouds[i]->size() > 0) {
//                            sensor_msgs::PointCloud2 cloudglobalMsg;
//                            pcl::toROSMsg(*wheel.m_clouds[i], cloudglobalMsg);
//                            cloudglobalMsg.header.stamp = ros::Time::now();
//                            cloudglobalMsg.header.frame_id = "/map";
//                            g_cluster_publisher[i].publish(cloudglobalMsg);
//                        }
//                    }
//                }
//                std::cout << " cloud in size : " << cloud_filtered->size() << std::endl;
//                if (cloud_filtered->size() > 0) {
//                    sensor_msgs::PointCloud2 cloudglobalMsg;
//                    pcl::toROSMsg(*cloud_filtered, cloudglobalMsg);
//                    cloudglobalMsg.header.stamp = ros::Time::now();
//                    cloudglobalMsg.header.frame_id = "/map";
//                    g_src_publisher.publish(cloudglobalMsg);
//                }
//            }
            ros::spinOnce();
            loop_rate.sleep();
        }
    }

    std::cout<<" exit ..."<<std::endl;
    return 0;
}
