ROS Navigation - Publishing Sensor Stream

차드마·2021년 8월 8일
0

Publishing Sensor Streams over ROS

  • Navigation Stack이 안전하게 구동되기 위해서는 ROS 환경에서 sensor 값을 올바르게 publish하는것이 중요하다. 만일 Navigation stack이 센서로부터 정보를 전달받지 못한다면 정상적인 주행은 어려울 것이다.

  • Navigation Stack에 정보를 제공할 수 있는 센서들은 다양하지만, 현재 navigation stack은 sensor_msgs/LaserScan 메세지 타입이나 sensor_msgs/PointCloud 메세지 타입을 통해 publish 된 센서 데이터만을 수용할 수 있다.

ROS Message Headers

  • sensor_msgs/LaserScan 과 sensor_msgs/PointCloud 메세지 타입은 다른 ROS 메세지 타입처럼 tf frame과 time dependent information을 가지고 있다.

  • 이 정보들이 전달되는 방식을 규격화 하기 위해, 우리는 위 메세지들의 field로서 Header 메세지 타입을 이용한다.

  • Header 타입의 세가지 field는 아래와 같다.
#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 field는 Message가 주어진 publisher로 부터 전달되었을때마다 자동으로 그 값이 증가하는 identifier의 역할을 수행하는 field이다.

  • stamp field는 메세지 속 데이터와 연계되어야하는 time infomration을 가지고 있는 field이다. laser scan의 경우를 예로 들자면, stamp는 scan이 일어났을때의 시각에 해당할 것이다.

  • frame_id field는 메세지 속 데이터와 연계되어야 하는 tf frame information을 가지고 있는 field이다. laser scan의 경우를 생각하면, frame_id는 scan이 일어나는 frame에 해당할 것이다.

Publishing LaserScans over ROS

The LaserScan Message

  • ROS는 LaserScan이라는 특별한 message 타입을 제공한다. 이는 sensor_msgs package에 존재하며, scan으로 부터 얻은 정보를 hold할수 있게 해준다.

  • Scanner로부터 얻은 데이터를 message에 format 시킬수만 있다면, LaserScan 메세지는 거의 모든 laser 센서를 기존 코드와 연관시켜 이용할 수 있게 해준다.

  • 이러한 message를 생성하여 publish하기 전에, message 자체를 한번 들여다 보자.
#
# 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]
  • 대부분의 field는 이름만으로도 어떤 field인지 감을 잠을 수 있을 것이다.
  • 간단한 예시를 통해 좀 더 자세히 알아보자

Writing Code to Publish a LaserScan Message

  • ROS 환경에서 LaserScan message를 publish하는 것은 꽤나 직관적이다.

    아래 코드를 살펴보자
#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>

전송시킬 message인 sensor_msgs/LaserScan을 include 하자

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

이 코드는 ros::Publisher를 생성하는 코드이다. 이는 나중에 LaserScan Message를 ROS환경에서 보내는데에 사용될 것이다.

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

Scan의 값을 채우기 위해 임의의 더미 데이터를 채워넣는다. 실제로 로봇에 적용할 때는 laser driver에서 이러한 데이터값을 추출하여야 할 것이다. 

  //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];
 }

scan_msgs::LaserScan message를 생성하고 위에서 생성한 데이터를 채워넣는다

    scan_pub.publish(scan);

마지막으로 해당 message를 ROS를 통해 publish하자

Publishing PointClouds over ROS

The PointCloud Message

  • 현실 세계의 여러 point들에 대한 정보를 보관하고 공유하기위해 ROS는 sensor_msgs/PointCloud message를 제공한다.

  • 이 message는 3차원 세계의 point 들로 이루어진 배열을 channel처럼 보관된 관련정보와 함께 support하도록 만들어진 message이다.

  • cloud 속 각각의 point에 대한 intensity 값을 가지고있는 "intensity" channel과 함께 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();
  }
}

코드를 하나씩 살펴보면

#include <sensor_msgs/PointCloud.h>

sensor_msgs/PointCloud message를 include하고

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

PointCloud message를 보낼 ros::Publisher를 생성한다

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

frame 정보와 timestamp 정보로 PointCloud message의 헤더를 채우고

    cloud.points.resize(num_points);

더미 데이터를 채워넣을 point cloud의 개수를 정한다

    //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);

"intensity"라는 이름의 channel을 추가하고, 그 크기를 cloud에 만들어줄 point의 개수로 resize해준다.

    //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 메세지와 intensity channel을 더미 데이터로 채워넣자

    cloud_pub.publish(cloud);

마지막으로 PointCloud를 ROS 환경을 통해 publish하자

profile
차드마
초보 개발자
이전 포스트

ROS Navigation - tf

다음 포스트

Pid control tutorial

0개의 댓글