レーザSLAM放棄ガイドライン3:LaserScanデータ転送PCLとPiontCloudデータを実現

参照リンク:1.李太白lxシリーズの文章はゼロから2次元レーザーSLAM linkをかけます.歳月神偷小拳link link
PCLデータ型
PointXYZ
PointXYZは、3次元xyz座標情報のみを含むため、記憶位置合わせを満たすために1つの浮動小数点数を付加する最も一般的なポイントデータ型である、points[i]を利用することができる.Data[0]、またはpoints[i].xアクセスポイントのx座標値.

union
{
     
   float data[4];
   struct
   {
     
      float x;
      float y;
      float z;
   };
};
;

PointCloud
PCLで最も基本的なデータ型はPointCloudで、C++クラスです.Width(int)は、無秩序な点群のうち、点群の数を指定します.1行の点群の数を指定します.
height(int)は、秩序化されたポイントクラウド内のポイントクラウド行の数を指定します.無秩序な点群の場合、heightを1(widthは点群の大きさ)に設定して、点群が秩序化されているかどうかを区別します.
points(std::vector)は、すべての格納されたポイントクラウド(Point)の配列を含む.点群にXYZのデータが含まれている場合、pointsのデータ構造はpcl::PointXYZ

pcl::PointXYZ pointT;
pcl::PointCloud<pointT> pointcloudT;
std::vector<pcl::PointXYZ> data = cloud->points;

is_dense(bool)は、点群内のすべてのデータが限られているか、またはその中のいくつかの点が限られていないかを指定し、それらのXYZ値はinf/NaNのような値(false)を含む可能性がある.
sensor_origin_(Eigen::Vector 4 f)センサの採取姿勢(origin/translation)を指定するこのメンバーは通常オプションであり、PCLの主流アルゴリズムでは使用できない.
sensor_orientaion_(Eigen::Quaternionf)センサの採取位置(方向)を指定します.このメンバーは通常オプションであり,PCLの主流アルゴリズムでは使用できない.開発を簡単にするために、PointCloudクラスには多くのヘルプメンバー関数が含まれています.たとえば、点群が秩序化されているかどうかを判断するには、heightが1に等しいかどうかを確認する必要はありません.isOrganized()関数を使用して判断します.
重点を置く
1.sensor_msgs/scanとsensor_msgs/PointCloud 2はレーザデータの2つの表現形式であり、間はpclによって互いに変換することができる.ここではpclに依存するrosおよびpcl_conversionsは、scanデータをPointCloud 2データ3に変換する.ここでrosがPointCloudデータを公開するとき、pcl::PointCloud-->sensor_を作ってくれました.msgs::PointCloud 2なので、ここではsensor_msgs::scan --> pcl::PointCloud 4.scanのデータ形式は極座標に似ており、PointCloudのデータ形式はデカルト座標に似ている.

float range= scan_msg->ranges[i];
float angle =scan_msg->angle_increment*i+scan_msg->angle_min;
PointT & point_temp = cloud_msg->points[i];
point_temp.x=range * cos(angle);
point_temp.y=range * sin(angle);
point_temp.z=0.0;

float angle =scan_msg->angle_increment*i+scan_msg->angle_min;

同時に注意:私たちはできるだけ初期化する時、私有ネーミング空間の下のハンドルを使用して、このようにlaunchからロードしたパラメータを取得するために使用することができて、~に初期化して、彼を私有ネーミング空間の下のハンドルに初期化します.

ScanToPointCloud2Converter::ScanToPointCloud2Converter() : private_node_("~")

ソースコード
launchファイル

<launch>
<arg name ="bag_name" default="/home/csy/desktop/test_code/slam_csdn/data/lesson1.bag"/>
 <!--   bag     -->
<param name = "use_sim_teme " value="true"/>
<node name = "lesson2" pkg = "lesson2" type = "lesson2" output = "screen">
 <node name = "rviz" pkg="rviz" type="rviz" required="false" args=" -d /home/csy/desktop/test_code/slam_csdn/data/lesson.rviz"/>
<node name = "playbag" pkg="rosbag" type="play"
args="--clock  $(arg bag_name)"/>
</launch>

.hファイル

#include "ros/ros.h"
#include"float.h"
#include"sensor_msgs/LaserScan.h"
#include"pcl_ros/point_cloud.h"
#include <pcl/point_types.h>//    PointT        （    pcl::PointXYZ）。
#include "pcl_conversions/pcl_conversions.h"
class scan_to_pointcloud2_converter
{
     
private:
typedef pcl::PointXYZ pointT;
typedef pcl::PointCloud<pointT> pointcloudT;
    ros::NodeHandle scan_nh_;
    ros::Subscriber laser_subscriber_;
    ros::Publisher point_publish_;
    pointT invalid_value_;
    void ScanCall(const sensor_msgs::LaserScan::ConstPtr scan_msg);
public:
    scan_to_pointcloud2_converter(/* args */);
    ~scan_to_pointcloud2_converter();
};

.cppファイル

#include "lesson2/scan_to_pointcloud2_converter.h"
#include "limits"

scan_to_pointcloud2_converter::scan_to_pointcloud2_converter(/* args */){
     
    ROS_INFO("open ");
    laser_subscriber_ = scan_nh_.subscribe("laser_scan",100,&scan_to_pointcloud2_converter::ScanCall,this);
    point_publish_ = scan_nh_.advertise<pointcloudT>( "PointCloud",100 );
    invalid_value_.x = std::numeric_limits<float>::quiet_NaN();
    invalid_value_.y = std::numeric_limits<float>::quiet_NaN();
    invalid_value_.z = std::numeric_limits<float>::quiet_NaN();
}

scan_to_pointcloud2_converter::~scan_to_pointcloud2_converter(){
     
}
void scan_to_pointcloud2_converter::ScanCall(const sensor_msgs::LaserScan::ConstPtr scan_msg){
     
    pointcloudT::Ptr cloud_msg=boost::shared_ptr<pointcloudT>(new pointcloudT());
    int scan_size = scan_msg->ranges.size();
    cloud_msg->points.resize(  scan_size );
    for(int i=0;i<scan_size;i++){
     
        pointT & point_temp = cloud_msg->points[i];
        float range = scan_msg->ranges[i];
        if( !std::isfinite(range)){
     
            point_temp = invalid_value_;
            continue;
        }else if( range>scan_msg-> range_min && range<scan_msg->range_max){
     
            float angle = scan_msg->angle_min + i*(scan_msg->angle_increment);
            point_temp.x = range*cos(angle);
            point_temp.y = range*sin(angle);
            point_temp.z = 0.0;
           
        }else{
     
              point_temp = invalid_value_;
        }
    }
    cloud_msg->height = 1;
    cloud_msg->width = scan_size;
    cloud_msg->is_bigendian = false;
    pcl_conversions::toPCL(scan_msg->header, cloud_msg->header );  //  pcl_conversions
    point_publish_.publish(cloud_msg);

}
int main(int argc,char ** argv){
     
    ros::init(argc,argv,"lesson3");
    scan_to_pointcloud2_converter test_scan_to_point;
    ros::spin();
    return 0;
}