// #include <ros/ros.h>
// #include <tf2_ros/transform_listener.h>
// #include <geometry_msgs/TransformStamped.h>

ros::init(argc, argv, "tf_node");
ros::NodeHandle nh;
tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);
ros::Publisher pub = nh.advertise<geometry_msgs::TransformStamped>("transform",10);
ros::Rate rate(400);
while(nh.ok())
{
    geometry_msgs::TransformStamped trans;
    try {
        ros::Time now = ros::Time::now();
       // if(buff.canTransform("base_footprint","imu",now,
        // ros::Duration(0.03),NULL)
        // 30毫秒应该足够了
        trans = buff.lookupTransform("base_footprint",
                         "imu", now, 
                         ros::Duration(0.03) );
       // else
           // ROS_WARN("no transform in buffer");
    } catch (tf2::TransformException & ex) {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }
    pub.publish(trans);
    rate.sleep();
}

geometry_msgs::PointStamped point_in, point_out;
point_in.header.frame_id = "child";
point_in.header.seq = 0;
point_in.header.stamp = ros::Time(0);
point_in.point.x = 2;
point_in.point.y = 1;
point_in.point.z = 4;

// buff跟上面的使用一样， 返回的point_out是point_in在 parent坐标系 的坐标
point_out = buff.transform(point_in, point_out, "parent", ros::Duration(1));

ros::init(argc, argv, "test_tf");
ros::NodeHandle nh;

tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);
geometry_msgs::TransformStamped transform =
        buff.lookupTransform("parent","child",ros::Time(0),ros::Duration(1));

ROS_INFO("transform from parent to child  x: %f",transform.transform.translation.x);
ROS_INFO("transform from parent to child  y: %f",transform.transform.translation.y);
ROS_INFO("transform from parent to child  z: %f\n",transform.transform.translation.z);

geometry_msgs::PointStamped point_in, point_out;
point_in.header.frame_id = "child";
point_in.header.seq = 0;
point_in.header.stamp = ros::Time(0);
// 在 child 坐标系中的点
point_in.point.x = 2;
point_in.point.y = 1;
point_in.point.z = 4;

ROS_INFO("point in child: 2, 1, 4\n");
point_out = buff.transform(point_in, point_out, "parent", ros::Duration(1));

ROS_INFO("point int parent, x: %f",point_out.point.x);
ROS_INFO("point int parent, y: %f",point_out.point.y);
ROS_INFO("point int parent, z: %f\n",point_out.point.z);

cout<<fixed<<setprecision(2);

Eigen::Quaterniond quaternion(transform.transform.rotation.w,
                              transform.transform.rotation.x,
                              transform.transform.rotation.y,
                              transform.transform.rotation.z
                              );
Eigen::Matrix3d rotation = quaternion.matrix();
cout << "rotation matrix: " <<endl<< rotation <<endl<<endl;

Eigen::Matrix4d m;
m.block(0,0,3,3) = rotation;

Eigen::Vector4d Vcol, Vrow;
Vcol << transform.transform.translation.x,
        transform.transform.translation.y,
        transform.transform.translation.z, 1;
m.col(3) = Vcol;

Vrow << 0,0,0,1;
m.row(3) = Vrow.transpose();
cout << "transform matrix: "<<endl<< m<<endl<<endl;

Eigen::Vector4d point;
point << point_in.point.x,
         point_in.point.y,
         point_in.point.z, 1;

Eigen::Vector4d transformed_point = m * point;
cout << "transformed point: "<<endl<<transformed_point.head(3) << endl;

void tf2::Transform::setIdentity()
{
    // Matrix3x3 m_basis;
    m_basis.setIdentity();   // 单位矩阵
    // Vector3   m_origin;
    m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0));
}

Transform::operator*(const Transform& t) const
{
    return Transform(m_basis * t.m_basis, 
               (*this)(t.m_origin)  );
}

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//滤波的头文件
#include <pcl/filters/voxel_grid.h>

ros::Publisher pub;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);

  // ROS消息转化为PCL中的点云数据格式
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  pcl::PCLPointCloud2 cloud_filtered;   //存储滤波后的数据格式
  // 进行一个滤波处理
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor; //创建滤波对象
  sor.setInputCloud (cloudPtr);  //设置输入的滤波，将需要过滤的点云给滤波对象
  sor.setLeafSize (0.1, 0.1, 0.1);  //设置滤波时创建的体素大小为1cm立方体
  sor.filter (cloud_filtered);//执行滤波处理，存储输出cloud_filtered

  // 再将滤波后的点云的数据格式转换为ROS下的数据格式发布
  sensor_msgs::PointCloud2 output;
  pcl_conversions::moveFromPCL(cloud_filtered, output);//第一个参数是输入，后面的是输出
  pub.publish (output);
}

int main (int argc, char** argv)
{
  ros::init (argc, argv, "filter_cloud");//声明节点的名称
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/camera/depth/points", 1, cloud_cb);
  pub = nh.advertise<sensor_msgs::PointCloud2> ("filtered_cloud", 1);

  ros::spin ();
}

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

int main(int argc, char** argv)
{
    
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  pcl::PCDReader reader;
  reader.read("16line.pcd", *cloud);

  std::cerr << "Cloud before filtering: " << cloud->points.size() << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered3(new pcl::PointCloud<pcl::PointXYZ>);

  // Create the filtering object 直通滤波
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud(cloud);
  pass.setFilterFieldName("x");   // 设置操作的坐标轴
  pass.setFilterLimits(-5.0, 5.0);
  pass.filter(*cloud_filtered2);
  // filter range Y-axis
  // 注意多次滤波需要再次输入点云和调用filter函数
  pass.setInputCloud(cloud_filtered2);
  pass.setFilterFieldName("y");
  pass.setFilterLimits(-5.0, 5.0);
  pass.filter(*cloud_filtered3);

  std::cerr << "Cloud after filtering: " << cloud_filtered3->points.size() << std::endl;

  // 读取pcd点云数据进行操作，最后保存为pcd文件
  pcl::PCDWriter writer;
  writer.write("16line_filtered.pcd", *cloud_filtered3, false);

  return 0;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);      //待滤波点云
if (pcl::io::loadPCDFile("test.pcd", *cloud) < 0)
{
  PCL_ERROR("\a点云文件不存在！\n");
  system("pause");
  return -1;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); //滤波后点云
pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror; //创建滤波器对象
ror.setInputCloud(cloud);           //设置待滤波点云
ror.setRadiusSearch(0.02);            //设置查询点的半径邻域范围
ror.setMinNeighborsInRadius(5);         //设置判断是否为离群点的阈值，即半径内至少包括的点数
//ror.setNegative(true);            //默认false，保存内点；true，保存滤掉的外点
ror.filter(*cloud_filtered);          //执行滤波，保存滤波结果于cloud_filtered

void radiusremoval(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, double radius, int min_pts)
{
  pcl::KdTreeFLANN<pcl::PointXYZ> tree;
  tree.setInputCloud(cloud);
  vector<float> avg;
  for (int i = 0; i < cloud->points.size(); ++i)
  {
    vector<int> id(min_pts);
    vector<float> dist(min_pts);
    tree.nearestKSearch(cloud->points[i], min_pts, id, dist);
    // 是否是平方
    if (dist[dist.size() - 1] < radius)
    {
      cloud_filtered->push_back(cloud->points[i]);
    }
  }
}

#include <pcl/io/pcd_io.h>  //文件输入输出
#include <pcl/point_types.h>  //点类型相关定义
#include <pcl/visualization/cloud_viewer.h>  //点云可视化相关定义
#include <pcl/filters/statistical_outlier_removal.h>  //滤波相关
#include <pcl/common/common.h>  

// 1.读取点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader r;
r.read<pcl::PointXYZ>("table_scene_lms400.pcd", *cloud);
cout << "there are " << cloud->points.size() << " points before filtering." << endl;

// 2.统计滤波
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);             // K近邻搜索点个数
sor.setStddevMulThresh(1.0); // 标准差倍数
sor.setNegative(false);     // 保留未滤波点（内点）
sor.filter(*cloud_filter);  // 保存滤波结果到cloud_filter

// 3.滤波结果保存
pcl::PCDWriter w;
w.writeASCII<pcl::PointXYZ>("table_scene_lms400_filter.pcd", *cloud_filter);
cout << "there are " << cloud_filter->points.size() << " points after filtering." << endl;

system("pause");
return 0;

if (!initialized_)
{
    ros::NodeHandle private_nh("~/" + name);
    costmap_ = costmap;
    frame_id_ = frame_id;   //costmap_ros->getGlobalFrameID()

    unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
    // 参数赋值, 默认false
    private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
    if(!old_navfn_behavior_)
        convert_offset_ = 0.5;
    else
        convert_offset_ = 0.0;

    bool use_quadratic;   // 二次方的
    private_nh.param("use_quadratic", use_quadratic, true);

    // p_calc_声明为PotentialCalculator指针
    // QuadraticCalculator是它的派生类, 唯一区别是覆盖了calculatePotential函数
    if (use_quadratic)    // 默认使用二次
        p_calc_ = new QuadraticCalculator(cx, cy);
    else
        p_calc_ = new PotentialCalculator(cx, cy);

    bool use_dijkstra;
    private_nh.param("use_dijkstra", use_dijkstra, true);
    //DijkstraExpansion 和 AStarExpansion 都继承 Expander类
    if (use_dijkstra)
    {
        DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
        if(!old_navfn_behavior_)
            de->setPreciseStart(true);
        planner_ = de;      // Expander* planner_;
    }
    else   // 如果不用dijkstra,就用A星算法.
        planner_ = new AStarExpansion(p_calc_, cx, cy);

    bool use_grid_path;
    private_nh.param("use_grid_path", use_grid_path, false);
    // GradientPath 和 GridPath都继承了Traceback
    if (use_grid_path)
        path_maker_ = new GridPath(p_calc_);
    else
        path_maker_ = new GradientPath(p_calc_);
        
    orientation_filter_ = new OrientationFilter();
    // NavfnROS::initialize中也注册此话题
    plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
    potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
    // 一系列参数赋值
    private_nh.param("allow_unknown", allow_unknown_, true);
    planner_->setHasUnknown(allow_unknown_);
    private_nh.param("planner_window_x", planner_window_x_, 0.0);
    private_nh.param("planner_window_y", planner_window_y_, 0.0);
    private_nh.param("default_tolerance", default_tolerance_, 0.0);
    private_nh.param("publish_scale", publish_scale_, 100);

    double costmap_pub_freq;
    private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);

    //get the tf prefix
    ros::NodeHandle prefix_nh;
    tf_prefix_ = tf::getPrefixParam(prefix_nh);

    make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

    dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
    dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
            &GlobalPlanner::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);

    initialized_ = true;
} else
    ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");