Tare_Planner小项目----＞2D雷达的数据接口(一)3转2转伪3

源代码中采用的是VLP16激光雷达,它发布的数据在environment里面进行了处理

目标:把16线雷达截取出来一部分,压缩成2维,再z轴上复制堆叠成伪3维

一.接口断裂处:

        1.tare_planner_node订阅的"/registered_scan"在yaml里暂时替换成别的=>"/pcl_16to2D_scan"(这个是最后面数据处理完毕后发布的话题)

        2.注释掉tare当中不必要的话题订阅(在sensor_coverage_planner_ground.cpp)

// exploration_start_sub_ =
  //     nh.subscribe(pp_.sub_start_exploration_topic_, 5, &SensorCoveragePlanner3D::ExplorationStartCallback, this);     //这个订阅未被使用
  registered_scan_sub_ =
      nh.subscribe(pp_.sub_registered_scan_topic_, 5, &SensorCoveragePlanner3D::RegisteredScanCallback, this);//******三D相机********/registered_scan
        //EXPL 1.每帧执行pd_.planning_env_的一个操作 => :上传一次机器人位置和pd_.registered_cloud_->cloud_    2. 每5帧上传kepose ==>> 当前机器人位置"keypose_.pose.pose.position；pd_.keypose_.pose.covariance；pd_.cur_keypose_node_ind_" 和  "pd_.keypose_cloud_->cloud_",    
  // terrain_map_sub_ = nh.subscribe(pp_.sub_terrain_map_topic_, 5, &SensorCoveragePlanner3D::TerrainMapCallback, this);//局部地势/terrain_map
  //       //EXPL 1.将接收到的点,intensity达到阈值的都赋值给pd_.terrain_collision_cloud_->cloud_->points (只有这件事情)        (这个是局部地势)
  // terrain_map_ext_sub_ =
  //     nh.subscribe(pp_.sub_terrain_map_ext_topic_, 5, &SensorCoveragePlanner3D::TerrainMapExtCallback, this);//更大的地势/terrain_map_ext
  //       //EXPL 1.将接收到的点,intensity达到阈值的都赋值给pd_.terrain_ext_collision_cloud_             (只有这件事情)        (这个是较为全局的地势)
  state_estimation_sub_ =
      nh.subscribe(pp_.sub_state_estimation_topic_, 5, &SensorCoveragePlanner3D::StateEstimationCallback, this);//位置估计/state_estimation_at_scan
        //EXPL 1.跟机器人的位置有关,将位置信息赋值给pd_.initial_position_,(只初始化一次)   2.每次更新yaw(翻译是偏离航线)  3.每次更新机器人位置
        //MYTODO  地势的都是在   SensorCoveragePlanner3D::UpdateViewPoints()  这个函数里使用；；如果加入2D想要放弃地势变化的话看看怎么改能让这个函数正常工作
  // coverage_boundary_sub_ =                                                                                               //这3个订阅未被使用
  //     nh.subscribe(pp_.sub_coverage_boundary_topic_, 1, &SensorCoveragePlanner3D::CoverageBoundaryCallback, this);
  // viewpoint_boundary_sub_ =
  //     nh.subscribe(pp_.sub_viewpoint_boundary_topic_, 1, &SensorCoveragePlanner3D::ViewPointBoundaryCallback, this);
  // nogo_boundary_sub_ =
  //     nh.subscribe(pp_.sub_nogo_boundary_topic_, 1, &SensorCoveragePlanner3D::NogoBoundaryCallback, this);

        3.把kCheckTerrainCollision在yaml里默认改为false(取消地势分析)

二.承接/registered_scan重新发布,重接伤口

        完整代码

#include <ros/ros.h>
#include <iostream>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

#include <pcl/filters/passthrough.h>
class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("/registered_scan", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_16to2D_scan", 1);
    }
    void cloudCB(const sensor_msgs::PointCloud2 &input)                                 
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(input, cloud);
        //
        pcl::PointCloud<pcl::PointXYZ>::Ptr pcloud (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PassThrough<pcl::PointXYZ> pass;// 创建滤波器对象
        pcl::copyPointCloud(cloud, *pcloud);
        pass.setInputCloud (pcloud);//设置输入点云
        pass.setFilterFieldName ("z");//滤波字段名被设置为Z轴方向
        pass.setFilterLimits (0.2, 1.4);//可接受的范围为（0.0，1.0） 
        //pass.setFilterLimitsNegative (true);//设置保留范围内 还是 过滤掉范围内
        pass.filter (*cloud_filtered); //执行滤波，保存过滤结果在cloud_filtered
        //
        /*这段将点云z设置为统一值*/
        for(int i=0;i<cloud_filtered->width;i++)
        {
            cloud_filtered->points[i].z=0.8;
        }
        /*这段给点云复制,多增加立体感beg*/
        pcl::PointCloud<pcl::PointXYZ> cloud_add;
        pcl::copyPointCloud(*cloud_filtered, cloud_add);
        for(int h=0;h<=20;h+=2)
        {
            for(int i=0;i<cloud_add.width;i++)
            {
                cloud_add.points[i].z=(float)h/10;
            }
            // std::cout<<"cloud_add.points[i].z: "<<(float)h/10<<std::endl;
            *cloud_filtered+=cloud_add;
        }
        /*这段给点云复制,多增加立体感end*/
        sensor_msgs::PointCloud2 cloud_filtered_pu;
        pcl::toROSMsg(*cloud_filtered, cloud_filtered_pu);                       //1.数据  2.时间戳
        cloud_filtered_pu.header.stamp = input.header.stamp;
        cloud_filtered_pu.header.frame_id = input.header.frame_id;
        pcl_pub.publish(cloud_filtered_pu);
        //
    }
protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Publisher pcl_pub;
};
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_visualize");
    cloudHandler handler;
    ros::spin();
    return 0;
}

cmakelist.txt

add_executable(3d_to_2d_scan src/3d_to_2d_scan/3d_to_2d_scan.cpp)
target_link_libraries(3d_to_2d_scan ${catkin_LIBRARIES}  ${PCL_LIBRARIES})

三.点云数据的改变过程(实际效果)

图1.上面这个是原点云数据,下面这张由上面这张截出几条线而成

 

图2.把图1中的下面这张点云压缩到2D

 图3.z周上复制堆叠增加立体感

最后再把这个点云"cloud_filtered"发布到"/pcl_16to2D_scan"这个话题中,tare将使用这个话题的数据

sensor_msgs::PointCloud2 cloud_filtered_pu;
pcl::toROSMsg(*cloud_filtered, cloud_filtered_pu);             //1.数据  2.时间戳
cloud_filtered_pu.header.stamp = input.header.stamp;
cloud_filtered_pu.header.frame_id = input.header.frame_id;     //传向的frame
pcl_pub.publish(cloud_filtered_pu);

此时rqt_graph中可见,tare_planner_node不再订阅"/registered_scan",转而订阅"/pcl_16to2D_scan"