【cartographer |
您所在的位置：网站首页 › ros订阅节点 › 【cartographer |
【cartographer
|
上一节介绍和测试了cartographer的官方demo。 本节会编写ros系统中，最常用的激光雷达LaserScan传感数据的订阅和发布，方便在cartographer中加入自己的数据进行建图与定位。(作者使用的是SICK-NAV350) 官方文档： http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors 目录 1：sensor_msgs/LaserScan消息类型 2：发布LaserScan消息 3：订阅LaserScan消息 1：sensor_msgs/LaserScan消息类型在终端查看消息数据结构： rosmsg show sensor_msgs/LaserScanLaserScan消息类型数据结构如下： 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 # range data [m] (Note: values < range_min or > range_max should be discarded) float32[] intensities # intensity data [device-specific units]. If your device does not provide intensities, please leave the array empty.得到了消息数据结构之后，就可以根据雷达数据填充消息内容。 其中angle_min angle_max angle_increment time_increment range_min range_max等基本上是雷达设置常量。 而ranges表示对应角度的测量距离，intensities表示对应角度的反射强度，这些是动态变化的，需要根据雷达频率更新。 2：发布LaserScan消息 #include #include int main(int argc, char** argv) { //ros节点初始化 ros::init(argc, argv, "laser_scan_publisher"); ros::NodeHandle n; //创建LaserScan消息发布Publisher ros::Publisher scan_pub = n.advertise("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()) { //为激光扫描生成一些假数据(可替换成自己激光雷达真实数据) for(unsigned int i = 0; i < num_readings; ++i) { ranges[i] = count; intensities[i] = 100 + count; } ros::Time scan_time = ros::Time::now(); //填充 LaserScan 消息 sensor_msgs::LaserScan scan; scan.header.stamp = scan_time; scan.header.frame_id = "base_link"; 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]; } //通过 ROS 发布消息 scan_pub.publish(scan); ++count; r.sleep(); } } 3：订阅LaserScan消息通常来说，发布自己激光雷达的信息到ros系统中，同时cartographer节点能够自动订阅接收LaserScan消息，就可以实现将雷达数据传给cartographer算法的目的。 但特定情况下可能需要订阅查看激光雷达的信息。 (1) 通过rosbag订阅 rostopic echo /scan(2) 通过rviz查看 打开rviz rosrun rviz rvizFixed Frame修改为base_link，添加LaserScan并将Topic设为/scan (3) 编写程序打印 #include "ros/ros.h" #include "sensor_msgs/LaserScan.h" void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg) { size_t size = msg->ranges.size(); std::string ranges =""; for(size_t i =0; i< size;i++){ ranges += std::to_string(msg->ranges[i]) + ", "; } ROS_INFO("Scan: [%s]", ranges); } int main(int argc, char **argv) { ros::init(argc, argv, "listener"); ros::NodeHandle node; ros::Subscriber subScan = node.subscribe("scan", 1000, ScanCallback); ros::spin(); return 0; }【完】 下一节会介绍里程计Odom数据的发布和订阅。 |
【本文地址】
今日新闻 |
推荐新闻 |