ROS Navigation - Publishing Sensor Stream

Publishing Sensor Streams over ROS

ナビゲーションワークステーションの安全な動作を確保するためには、ROS環境においてセンサ値を正確に発行することが重要である.ナビゲーションスタックがセンサから情報を受信できない場合は、正常に動作しにくい.

Navigation Stackに情報を提供できるセンサは多いが、現在のナビゲーションスタックではsensor msgs/Laserscanメッセージタイプまたはsensor msgs/PointCloudメッセージタイプを介して発行されるセンサデータしか受け入れられない.

ROS Message Headers

センサmsgs/LaserScanおよびセンサmsgs/PointCloudメッセージタイプは、他のROSメッセージタイプと同様にtfフレームおよび時間に関する情報を有する.

これらの情報の伝達方法を規範化するために,Headerメッセージタイプを上記メッセージのドメインとして用いた.

Headerタイプの3つのフィールドを以下に示します.

#Standard metadata for higher-level flow data types
#sequence ID: consecutively increasing ID 
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.secs: seconds (stamp_secs) since epoch
# * stamp.nsecs: nanoseconds since stamp_secs
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id

seqフィールドは、所与のpublisherからメッセージが伝達されるたびに、その値が増加する識別子ロールを自動的に実行するフィールドである.

stampfieldは、メッセージ内のデータに関連付けなければならない時間情報を持つフィールドです.レーザスキャンを例にとると、stampはスキャン発生時の時刻に相当する.

frame idフィールドは、メッセージ内のデータに関連付けなければならないtf frame情報を有するフィールドである.レーザスキャンの場合、frame idはスキャンが発生したフレームに相当する.

Publishing LaserScans over ROS
The LaserScan Message

ROSは、LaserScanと呼ばれる特殊なメッセージタイプを提供する.sensor msgs packageに存在し、scanから取得した情報を保存できます.

スキャナで得られたデータをメッセージにフォーマットできる限り、LaserScanメッセージは、ほとんどのレーザセンサを既存のコードに関連付けて使用することができる.

これらのメッセージを生成してパブリッシュする前に、メッセージ自体を見てみましょう.

#
# Laser scans angles are measured counter clockwise, with 0 facing forward
# (along the x-axis) of the device frame
#

Header header
float32 angle_min        # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]
float32 time_increment   # time between measurements [seconds]
float32 scan_time        # time between scans [seconds]
float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]
float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities    # intensity data [device-specific units]

のほとんどのフィールドは、名前だけで何のフィールドかを感じることができます.

簡単な例により

をさらに理解する.
Writing Code to Publish a LaserScan Message

ROS環境でLaserScanメッセージを公開するのはかなり直感的です.
次は

コードです

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "laser_scan_publisher");

  ros::NodeHandle n;
  ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

  unsigned int num_readings = 100;
  double laser_frequency = 40;
  double ranges[num_readings];
  double intensities[num_readings];

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    //generate some fake data for our laser scan
    for(unsigned int i = 0; i < num_readings; ++i){
      ranges[i] = count;
      intensities[i] = 100 + count;
    }
    ros::Time scan_time = ros::Time::now();

    //populate the LaserScan message
    sensor_msgs::LaserScan scan;
    scan.header.stamp = scan_time;
    scan.header.frame_id = "laser_frame";
    scan.angle_min = -1.57;
    scan.angle_max = 1.57;
    scan.angle_increment = 3.14 / num_readings;
    scan.time_increment = (1 / laser_frequency) / (num_readings);
    scan.range_min = 0.0;
    scan.range_max = 100.0;

    scan.ranges.resize(num_readings);
    scan.intensities.resize(num_readings);
    for(unsigned int i = 0; i < num_readings; ++i){
      scan.ranges[i] = ranges[i];
      scan.intensities[i] = intensities[i];
    }

    scan_pub.publish(scan);
    ++count;
    r.sleep();
  }
}

ひとつひとつ見る

#include <sensor_msgs/LaserScan.h>

送信するメッセージを含むsensor msgs/LaserScan

ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

このコードはros::Publisherを生成するコードです.これは、後でROS環境でLaserScanメッセージを送信するために使用されます.

  unsigned int num_readings = 100;
  double laser_frequency = 40;
  double ranges[num_readings];
  double intensities[num_readings];

Scanのデータの山を埋めます.実際には、ロボットに適用する場合、レーザドライバからこれらのデータ値を抽出する必要がある場合があります.

  //generate some fake data for our laser scan
  for(unsigned int i = 0; i < num_readings; ++i){
    ranges[i] = count;
    intensities[i] = 100 + count;
  }
  ros::Time scan_time = ros::Time::now();

増加したヒープ・データを1秒あたり埋め込む

 //populate the LaserScan message
 sensor_msgs::LaserScan scan;
 scan.header.stamp = scan_time;
 scan.header.frame_id = "laser_frame";
 scan.angle_min = -1.57;
 scan.angle_max = 1.57;
 scan.angle_increment = 3.14 / num_readings;
 scan.time_increment = (1 / laser_frequency) / (num_readings);
 scan.range_min = 0.0;
 scan.range_max = 100.0;

 scan.ranges.resize(num_readings);
 scan.intensities.resize(num_readings);
 for(unsigned int i = 0; i < num_readings; ++i){
   scan.ranges[i] = ranges[i];
   scan.intensities[i] = intensities[i];
 } 

スキャンmsgs::LaserScanメッセージを生成し、上で生成したデータを埋め込む

    scan_pub.publish(scan);

最後に、ROSを通じてこのメッセージを発表します.
Publishing PointClouds over ROS
The PointCloud Message
ROSは、現実世界の複数のポイントに関する情報を保存および共有するためにsensor msgs/PointCloudメッセージを提供する.

この情報は,3次元世界の点からなる配列とchannelのように保存された関連情報をサポートするために作成された.

[強度](Strength)チャネルを使用してPointCloudを転送する例を次に示します.このチャネルは、

クラウドの各ポイントの強度値を有します.

#This message holds a collection of 3d points, plus optional additional information about each point.
#Each Point32 should be interpreted as a 3d point in the frame given in the header

Header header
geometry_msgs/Point32[] points  #Array of 3d points
ChannelFloat32[] channels       #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point

Writing Code to Publish a PointCloud Message

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "point_cloud_publisher");

  ros::NodeHandle n;
  ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

  unsigned int num_points = 100;

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "sensor_frame";

    cloud.points.resize(num_points);

    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points);

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = 1 + count;
      cloud.points[i].y = 2 + count;
      cloud.points[i].z = 3 + count;
      cloud.channels[0].values[i] = 100 + count;
    }

    cloud_pub.publish(cloud);
    ++count;
    r.sleep();
  }
}

コードを1つずつ表示すると、

#include <sensor_msgs/PointCloud.h>

sensor msgs/PointCloudメッセージが含まれます.

ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

PointCloudメッセージを送信するros::Publisherを生成

    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "sensor_frame";

フレーム情報とタイムスタンプ情報を使用してPointCloudメッセージヘッダを埋めます.

    cloud.points.resize(num_points);

スタックデータを埋め込むpointcloudの個数を決定する

    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points);

強さ(Strength)という名前のチャネルを追加し、クラウドで作成するポイントの数にサイズを変更します.

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = 1 + count;
      cloud.points[i].y = 2 + count;
      cloud.points[i].z = 3 + count;
      cloud.channels[0].values[i] = 100 + count;
    }

スタックされたデータを使用してPointCloudメッセージと強度チャネルを埋め込む

    cloud_pub.publish(cloud);

最後に、ROS環境でPointCloudをリリース