【cartogarpher_ros】三: 发布和订阅雷达scan信息

人工智能88

上一节介绍和测试了cartographer的官方demo。

本节会编写ros系统中,最常用的激光雷达LaserScan传感数据的订阅和发布,方便在cartographer中加入自己的数据进行建图与定位。(作者使用的是SICK-NAV350)

官方文档:
http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors

目录

1:sensor_msgs/LaserScan消息类型

查看终端上的消息数据结构:

[En]

View the message data structure on the terminal:

rosmsg show sensor_msgs/LaserScan

LaserScan消息类型数据结构如下:

std_msgs/Header header          
  uint32 seq
  time stamp           
  string frame_id     # in frame frame_id, angles are measured around  the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis           
                         
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] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]

float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]

float32[] ranges &#xA0; &#xA0; &#xA0; &#xA0; # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities &#xA0; &#xA0;# intensity data [device-specific units]. &#xA0;If your device does not provide intensities, please leave the array empty.

一旦获得报文数据结构,就可以根据雷达数据填充报文内容。

[En]

Once the message data structure is obtained, the message content can be populated according to the radar data.

其中angle_min angle_max angle_increment time_increment range_min range_max等基本上是雷达设置常量。
而ranges表示对应角度的测量距离,intensities表示对应角度的反射强度,这些是动态变化的,需要根据雷达频率更新。

2:发布LaserScan消息

#include <ros ros.h>
#include <sensor_msgs laserscan.h>

int main(int argc, char** argv)
{
&#xA0;&#xA0; &#xA0;//ros&#x8282;&#x70B9;&#x521D;&#x59CB;&#x5316;
&#xA0; &#xA0; ros::init(argc, argv, "laser_scan_publisher");
&#xA0; &#xA0; ros::NodeHandle n;

&#xA0;&#xA0; &#xA0;//&#x521B;&#x5EFA;LaserScan&#x6D88;&#x606F;&#x53D1;&#x5E03;Publisher
&#xA0; &#xA0; ros::Publisher scan_pub = n.advertise<sensor_msgs::laserscan>("scan", 50);

&#xA0;&#xA0; &#xA0;//&#x6FC0;&#x5149;&#x96F7;&#x8FBE;&#x53C2;&#x6570;&#x8BBE;&#x7F6E;
&#xA0; &#xA0; unsigned int num_readings = 100;
&#xA0; &#xA0; double laser_frequency = 40;
&#xA0; &#xA0; double ranges[num_readings];
&#xA0; &#xA0; double intensities[num_readings];
&#xA0; &#xA0; int count = 0;

&#xA0; &#xA0; ros::Rate r(1.0);

&#xA0; &#xA0; while(n.ok())
&#xA0; &#xA0; {
&#xA0; &#xA0; &#xA0; &#xA0; //&#x4E3A;&#x6FC0;&#x5149;&#x626B;&#x63CF;&#x751F;&#x6210;&#x4E00;&#x4E9B;&#x5047;&#x6570;&#x636E;(&#x53EF;&#x66FF;&#x6362;&#x6210;&#x81EA;&#x5DF1;&#x6FC0;&#x5149;&#x96F7;&#x8FBE;&#x771F;&#x5B9E;&#x6570;&#x636E;)
&#xA0; &#xA0; &#xA0; &#xA0; for(unsigned int i = 0; i < num_readings; ++i)
&#xA0; &#xA0; &#xA0; &#xA0; {
&#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; ranges[i] = count;
&#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; intensities[i] = 100 + count;
&#xA0; &#xA0; &#xA0; &#xA0; }
&#xA0; &#xA0; &#xA0; &#xA0; ros::Time scan_time = ros::Time::now();
&#xA0;&#xA0; &#xA0;
&#xA0; &#xA0; &#xA0; &#xA0; //&#x586B;&#x5145; LaserScan &#x6D88;&#x606F;
&#xA0; &#xA0; &#xA0; &#xA0; sensor_msgs::LaserScan scan;
&#xA0; &#xA0; &#xA0; &#xA0; scan.header.stamp = scan_time;
&#xA0; &#xA0; &#xA0; &#xA0; scan.header.frame_id = "base_link";
&#xA0; &#xA0; &#xA0; &#xA0; scan.angle_min = -1.57;
&#xA0; &#xA0; &#xA0; &#xA0; scan.angle_max = 1.57;
&#xA0; &#xA0; &#xA0; &#xA0; scan.angle_increment = 3.14 / num_readings;
&#xA0; &#xA0; &#xA0; &#xA0; scan.time_increment = (1 / laser_frequency) / (num_readings);
&#xA0; &#xA0; &#xA0; &#xA0; scan.range_min = 0.0;
&#xA0; &#xA0; &#xA0; &#xA0; scan.range_max = 100.0;
&#xA0; &#xA0; &#xA0; &#xA0; scan.ranges.resize(num_readings);
&#xA0; &#xA0; &#xA0; &#xA0; scan.intensities.resize(num_readings);
&#xA0; &#xA0; &#xA0; &#xA0; for(unsigned int i = 0; i < num_readings; ++i)
&#xA0; &#xA0; &#xA0; &#xA0; {
&#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; scan.ranges[i] = ranges[i];
&#xA0; &#xA0; &#xA0; &#xA0; &#xA0; &#xA0; scan.intensities[i] = intensities[i];
&#xA0; &#xA0; &#xA0; &#xA0; }

&#xA0;&#xA0; &#xA0;&#xA0;&#xA0; &#xA0;//&#x901A;&#x8FC7; ROS &#x53D1;&#x5E03;&#x6D88;&#x606F;
&#xA0; &#xA0; &#xA0; &#xA0; scan_pub.publish(scan);

&#xA0; &#xA0; &#xA0; &#xA0; ++count;
&#xA0; &#xA0; &#xA0; &#xA0; r.sleep();
&#xA0; &#xA0; }
}
</sensor_msgs::laserscan></sensor_msgs></ros>

3:订阅LaserScan消息

通常来说,发布自己激光雷达的信息到ros系统中,同时cartographer节点能够自动订阅接收LaserScan消息,就可以实现将雷达数据传给cartographer算法的目的。
但是,在某些情况下,您可能需要订阅才能查看激光雷达信息。

[En]

However, under certain circumstances, you may need to subscribe to view lidar information.

(1) 通过rosbag订阅

rostopic echo /scan

(2) 通过rviz查看
打开rviz

rosrun rviz rviz

Fixed Frame修改为base_link,添加LaserScan并将Topic设为/scan
【cartogarpher_ros】三: 发布和订阅雷达scan信息

(3) 编写程序打印

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"

void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
&#xA0; &#xA0; size_t size = msg->ranges.size();
&#xA0; &#xA0; std::string ranges ="";
&#xA0; &#xA0; for(size_t i =0; i< size;i++){
&#xA0; &#xA0; &#xA0; &#xA0; ranges += std::to_string(msg->ranges[i]) + ", ";
&#xA0; &#xA0; }
&#xA0; &#xA0; ROS_INFO("Scan: [%s]", ranges);
}

int main(int argc, char **argv)
{
&#xA0; &#xA0; ros::init(argc, argv, "listener");
&#xA0; &#xA0; ros::NodeHandle node;
&#xA0; &#xA0; ros::Subscriber subScan = node.subscribe("scan", 1000, ScanCallback);
&#xA0; &#xA0; ros::spin();
&#xA0; &#xA0; return 0;
}

【完】

下一节会介绍里程计Odom数据的发布和订阅。

Original: https://www.cnblogs.com/CloudFlame/p/16443725.html
Author: CloudFlame
Title: 【cartogarpher_ros】三: 发布和订阅雷达scan信息