LaserScan数据转PointCloud2

通过stage_ros中rosbag record -a记录的数据

里面的坐标系分别是odom >> base_foot_print >> base_link >> base_laser_link

通过

rosrun tf tf_monitor

或者

rosrun tf view_frames

可查看

再通过rostopic查看激光雷达数据

header:
  seq: 27642
  stamp:
    secs: 552
    nsecs: 940000000
  frame_id: "base_laser_link"
angle_min: -3.14159274101
angle_max: 3.14159274101
angle_increment: 0.0175019092858
time_increment: 0.0
scan_time: 0.0
range_min: 0.0
range_max: 8.0
ranges: [2.3671581745147705, 2.437469005584717, 2.5450503826141357, 2.4730722904205322, 2.129777431488037, 2.6910219192504883, 2.5043976306915283, 2.6225624084472656, 2.5274298191070557, 2.6237497329711914, 2.4797120094299316, 3.0273640155792236, 2.8899130821228027, 2.9636499881744385, 3.0600106716156006, 3.0046639442443848, 3.222302198410034, 3.219674587249756, 3.293705701828003, 3.594900131225586, 3.7113006114959717, 3.6515579223632812, 4.093756198883057, 4.407590866088867, 4.07601261138916, 4.813910961151123, 4.912193775177002, 2.182116746902466, 2.268886089324951, 2.2959141731262207, 2.454716444015503, 2.446974277496338, 2.4393270015716553, 2.1425952911376953, 2.0264012813568115, 2.1251349449157715, 2.4029078483581543, 2.2355992794036865, 2.289663553237915, 1.9838523864746094, 2.4128284454345703, 2.1235573291778564, 2.283503532409668, 2.366795539855957, 2.0017309188842773, 2.227402687072754, 2.051004409790039, 1.9057296514511108, 2.011770009994507, 2.177748441696167, 2.0254697799682617, 2.080940008163452, 1.9965004920959473, 2.06980562210083, 2.267909526824951, 2.2184808254241943, 2.2705447673797607, 2.2309272289276123, 2.154033899307251, 2.3025667667388916, 2.2236969470977783, 2.170229434967041, 2.247009515762329, 2.0627248287200928, 2.308018445968628, 2.1607415676116943, 2.294560194015503, 2.4851155281066895, 2.2002761363983154, 2.4646806716918945, 2.3871922492980957, 2.361760139465332, 2.189034938812256, 2.1875367164611816, 2.277064323425293, 2.3127880096435547, 2.203293561935425, 2.634711503982544, 2.491438865661621, 2.3672358989715576, 2.661224603652954, 2.850015878677368, 2.556377410888672, 2.7326478958129883, 2.8596725463867188, 2.62313175201416, 2.5699377059936523, 2.725731611251831, 2.910217046737671, 2.914889097213745, 3.065117597579956, 3.2342300415039062, 3.0099942684173584, 3.0506927967071533, 2.881638288497925, 2.9063737392425537, 2.9763786792755127, 2.635496139526367, 2.799639940261841, 2.49661922454834, 2.821577310562134, 2.489149570465088, 2.344313144683838, 2.56527042388916, 2.3331000804901123, 2.2578790187835693, 2.54424786567688, 2.58272647857666, 2.464503526687622, 2.1933929920196533, 2.6992077827453613, 2.1864185333251953, 2.357841968536377, 2.184927225112915, 2.1834747791290283, 2.1896920204162598, 2.2782180309295654, 2.146247148513794, 2.4055442810058594, 2.2821948528289795, 2.3352973461151123, 2.147887706756592, 2.294842004776001, 2.207432270050049, 2.0835750102996826, 2.2948355674743652, 2.160386562347412, 2.2226781845092773, 2.0389983654022217, 2.088817596435547, 2.045210361480713, 1.956254482269287, 2.13010835647583, 2.0623819828033447, 2.142322301864624, 2.0434582233428955, 2.126939296722412, 1.8722319602966309, 1.9698251485824585, 2.150585174560547, 2.281700372695923, 2.0749666690826416, 1.9728561639785767, 2.0918173789978027, 2.0569798946380615, 2.090402841567993, 2.0717694759368896, 1.9337363243103027, 2.043487787246704, 2.0771026611328125, 2.0422048568725586, 2.263404607772827, 2.2125139236450195, 2.237689733505249, 2.365427255630493, 2.350411891937256, 2.231801748275757, 2.44431734085083, 2.2608959674835205, 2.1739251613616943, 2.1691641807556152, 2.0955607891082764, 2.146925687789917, 2.5238213539123535, 2.092698574066162, 2.4306862354278564, 2.497982978820801, 2.5041592121124268, 2.406712532043457, 2.5105233192443848, 2.617666006088257, 2.3595757484436035, 2.5710504055023193, 2.5886447429656982, 2.73690128326416, 2.6873600482940674, 2.8863909244537354, 2.9560976028442383, 2.7096729278564453, 2.9185752868652344, 2.599407434463501, 2.8948938846588135, 2.9630286693573, 2.9772088527679443, 3.175264835357666, 3.171440601348877, 3.076348066329956, 3.2406728267669678, 3.6811363697052, 3.2240359783172607, 3.3164966106414795, 3.5035653114318848, 3.5760159492492676, 3.593306064605713, 3.7063779830932617, 3.5152628421783447, 3.234682559967041, 3.5148444175720215, 3.547987222671509, 3.6434803009033203, 3.6482837200164795, 3.3774797916412354, 3.5490574836730957, 3.2515666484832764, 3.4863715171813965, 3.201115846633911, 3.4158737659454346, 3.0262773036956787, 3.2734880447387695, 3.2226924896240234, 3.2764525413513184, 3.176913022994995, 3.3980090618133545, 3.289044141769409, 3.159752368927002, 3.3576691150665283, 2.9783904552459717, 3.204716205596924, 3.114088535308838, 3.0238351821899414, 3.2746644020080566, 3.2566771507263184, 3.0854740142822266, 3.198639392852783, 3.127133369445801, 2.8885951042175293, 2.9361979961395264, 3.0978100299835205, 3.0293469429016113, 2.9818336963653564, 3.0325777530670166, 3.171574831008911, 2.867344617843628, 3.271404504776001, 3.118687868118286, 3.1062967777252197, 3.5527312755584717, 3.0607872009277344, 3.0730159282684326, 3.4444117546081543, 3.2924234867095947, 2.7469143867492676, 6.197441577911377, 6.024816989898682, 5.5790934562683105, 4.645350933074951, 4.625833511352539, 4.805953502655029, 4.1594672203063965, 4.456159591674805, 3.8483517169952393, 3.764951229095459, 4.210761070251465, 3.7496485710144043, 3.670624017715454, 3.5715134143829346, 3.5464909076690674, 3.2750742435455322, 3.411051034927368, 3.0112695693969727, 3.0902888774871826, 2.8161470890045166, 2.657740831375122, 2.8620779514312744, 2.576160430908203, 2.6139075756073, 2.639108180999756, 2.7044003009796143, 2.24112606048584, 2.3557660579681396, 2.304212808609009, 2.333465814590454, 2.250274181365967, 2.1604092121124268, 2.404167652130127, 2.261396884918213, 2.239363670349121, 2.0727782249450684, 2.098773241043091, 2.0071568489074707, 2.348360538482666, 1.8408020734786987, 2.134660243988037, 2.095571517944336, 2.107762575149536, 2.059391736984253, 1.8073879480361938, 1.801904320716858, 1.9037002325057983, 1.8997881412506104, 1.725953459739685, 1.9835577011108398, 1.9278467893600464, 1.6701762676239014, 1.6520766019821167, 1.8454875946044922, 1.9041157960891724, 1.6904313564300537, 1.8891605138778687, 1.8439339399337769, 1.7576066255569458, 1.8114045858383179, 1.6478726863861084, 1.7261539697647095, 1.6495989561080933, 1.7014068365097046, 1.8382560014724731, 1.766120433807373, 1.76628839969635, 1.6847593784332275, 1.721574306488037, 1.6688514947891235, 1.5659679174423218, 1.5292747020721436, 1.7208778858184814, 1.6612175703048706, 1.5912736654281616, 1.6257727146148682, 1.7246522903442383, 1.5915873050689697, 1.4988420009613037, 1.7843629121780396, 1.776216983795166, 1.6802901029586792, 1.594672441482544, 1.519517421722412, 1.76614511013031, 1.7410868406295776, 1.6858357191085815, 1.5404313802719116, 1.6695796251296997, 1.750333547592163, 1.7739590406417847, 1.7463444471359253, 1.6805298328399658, 1.7066787481307983, 1.8262064456939697, 1.7992057800292969, 1.7194466590881348, 1.7334262132644653, 1.9029452800750732, 1.8903051614761353, 1.8821866512298584, 1.848001480102539, 1.9143539667129517, 1.9488102197647095, 1.8091624975204468, 1.9119899272918701, 1.9298498630523682, 1.8950719833374023, 1.8753653764724731, 2.106137275695801, 2.0147414207458496, 2.171975612640381, 2.235895872116089, 2.110197067260742, 2.2408499717712402, 2.087888717651367, 2.402405261993408, 2.276033878326416]
intensities: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]

使用默认laser_geometry函数

用来发布map->odom之间的tf

//publish tf between world and odom
void My_Filter::poseCallback(const nav_msgs::Odometry::ConstPtr& msg){
  //std::cout<<"poseCallback begin!"<<std::endl;
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  //transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );
  transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0));
  //double theta = 2 * atan2(msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
  tf::Quaternion q;
  q.setRPY(0, 0, 0); 
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, msg->header.stamp, "map", "odom"));
}

 用来将Laserscan转成"odom"坐标系下的点云，如果odom无误差的话，那就是map或者world坐标系下数据

//convert from laserscan to point cloud
void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    //std::cout<<"scanCallback begin!"<<std::endl;
    sensor_msgs::PointCloud2 cloud; 
        if(!tfListener_.waitForTransform(
            scan->header.frame_id,
            "/odom",
            scan->header.stamp + ros::Duration().fromSec(scan->ranges.size()*scan->time_increment),
           ros::Duration(1))){
        return;
     }
    projector_.transformLaserScanToPointCloud("/odom", *scan, cloud, tfListener_);
    //pcl::PointCloud<pcl::PointXYZ> cloud_out;
    //pcl::fromROSMsg(cloud, cloud_out);
   
    point_cloud_publisher_.publish(cloud);
}