[转]ROS订阅激光数据
https://github.com/robopeak/rplidar_ros/blob/master/src/client.cpp
| /* | |
| * Copyright (c) 2014, RoboPeak | |
| * All rights reserved. | |
| * | |
| * Redistribution and use in source and binary forms, with or without | |
| * modification, are permitted provided that the following conditions are met: | |
| * | |
| * 1. Redistributions of source code must retain the above copyright notice, | |
| * this list of conditions and the following disclaimer. | |
| * | |
| * 2. Redistributions in binary form must reproduce the above copyright notice, | |
| * this list of conditions and the following disclaimer in the documentation | |
| * and/or other materials provided with the distribution. | |
| * | |
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" | |
| * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, | |
| * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR | |
| * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR | |
| * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, | |
| * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, | |
| * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; | |
| * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, | |
| * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR | |
| * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, | |
| * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | |
| * | |
| */ | |
| /* | |
| * RoboPeak LIDAR System | |
| * RPlidar ROS Node client test app | |
| * | |
| * Copyright 2009 - 2014 RoboPeak Team | |
| * http://www.robopeak.com | |
| * | |
| */ | |
| #include "ros/ros.h" | |
| #include "sensor_msgs/LaserScan.h" | |
| #define RAD2DEG(x) ((x)*180./M_PI) | |
| void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) | |
| { | |
| int count = scan->scan_time / scan->time_increment; | |
| ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count); | |
| ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max)); | |
| for(int i = 0; i < count; i++) { | |
| float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); | |
| ROS_INFO(": [%f, %f]", degree, scan->ranges[i]); | |
| } | |
| } | |
| int main(int argc, char **argv) | |
| { | |
| ros::init(argc, argv, "rplidar_node_client"); | |
| ros::NodeHandle n; | |
| ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback); | |
| ros::spin(); | |
| return 0; | |
| } |
