Lidarをtfで回転させて3D-PointCloudに変換する。

背景

　rosのlaser_assemblerノードで、Lidarを回転させて3D変換する方法について説明する。2DのLidarのデータをtfで座標変換させ、データを貯め込むことで、3DのPointCloudを作成する。

laser_assembler

　Lidarを回転する場合、Lidarの測定タイミングと回転機構の回転角度検知タイミングとの時間合わせが重要である。タイミングがあっていないと、検出された形状が歪んでしまう。
tfはサンプリングレートの異なるtopicを受信する際に、時間軸の補正を行う機能がある。
　tfとlaser_assemblerを使うことで、精度の良いPointCloudを生成することができる。
laser_assemblerは、既定の時間、tf変換されpointcloudのtopicを貯め込む機能を有する。

　laser_assemblerは、sensor_msgs/LaserScan・sensor_msgs/PointCloudトピックを受信し、tfで座標変換した後に、rolling bufferに格納する。rolling bufferは、serviceからの呼び出しによって、sensor_msgs/PointCloud形式での点群の吐き出しを行う。

laser_assemblerのインストール(ROS indigoの場合)

sudo apt-get install ros-indigo-laser-pipeline 
sudo apt-get install ros-indigo-laser-assembler 

laser_assemblerクライアントの作成

laser_assemblerはサービスを提供する。
laser_assemblerのexamples-periodic_snapshotter.cppを参考に、laser_assemblerクライアントを作成する。
laser_assemblerのexamplesは古く、"sensor_msgs/PointCloud"をpublishしているが、最近のROSノードは"sensor_msgs/PointCloud2"を使うことが多い。"sensor_msgs/PointCloud2"をpublishするように変更した。

参考：
https://github.com/ros-perception/laser_assembler/blob/hydro-devel/examples/periodic_snapshotter.cpp

periodic_snapshotter.cpp

#include <cstdio>
#include <ros/ros.h>

#include "laser_assembler/AssembleScans.h"

#include "sensor_msgs/PointCloud.h"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

namespace laser_assembler
{

class PeriodicSnapshotter
{

public:

  PeriodicSnapshotter()
  {
    pub_ = n_.advertise<sensor_msgs::PointCloud2> ("assembled_cloud", 1);
    client_ = n_.serviceClient<AssembleScans>("assemble_scans");
　　　//直近の5秒間の点群を出力する。
    timer_ = n_.createTimer(ros::Duration(5,0), &PeriodicSnapshotter::timerCallback, this);
    first_time_ = true;
  }

  void timerCallback(const ros::TimerEvent& e)
  {
    if (first_time_)
    {
      first_time_ = false;
      return;
    }
    AssembleScans srv;
    srv.request.begin = e.last_real;
    srv.request.end   = e.current_real;

    if (client_.call(srv))
    {
      ROS_INFO("Published Cloud with %u points", (uint32_t)(srv.response.cloud.points.size())) ;

      sensor_msgs::PointCloud2 cloud2;
      sensor_msgs::convertPointCloudToPointCloud2(srv.response.cloud, cloud2);
      pub_.publish(cloud2);
    }
    else
    {
      ROS_ERROR("Error making service call\n") ;
    }
  }

private:
  ros::NodeHandle n_;
  ros::Publisher pub_;
  ros::ServiceClient client_;
  ros::Timer timer_;
  bool first_time_;
} ;

}

using namespace laser_assembler ;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "periodic_snapshotter");
  ros::NodeHandle n;
  ROS_INFO("Waiting for [build_cloud] to be advertised");
  ros::service::waitForService("build_cloud");
  ROS_INFO("Found build_cloud! Starting the snapshotter");
  PeriodicSnapshotter snapshotter;
  ros::spin();
  return 0;
}

launchファイルで起動する

  <!-- ノード "laser_scan_assembler"の起動 --> 
  <node type="laser_scan_assembler" pkg="laser_assembler" name="my_assembler">
    <remap from="scan" to="tilt_scan"/>
    <param name="max_scans" type="int" value="400" />
    <param name="fixed_frame" type="string" value="base_link" />
  </node>
  <!-- ノード"periodic_snapshotter"の起動 -->
  <node pkg="periodic_snapshotter" type="periodic_snapshotter" name="periodic_snapshotter" respawn="false" output="screen" />
</launch>

参考：
http://wiki.ros.org/laser_assembler
http://wiki.ros.org/laser_assembler/Tutorials/HowToAssembleLaserScans
http://researchmap.jp/joohy91ok-2001408/#_2001408