client.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2014, RoboPeak
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without 
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  * 1. Redistributions of source code must retain the above copyright notice, 
00009  *    this list of conditions and the following disclaimer.
00010  *
00011  * 2. Redistributions in binary form must reproduce the above copyright notice, 
00012  *    this list of conditions and the following disclaimer in the documentation 
00013  *    and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
00017  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
00018  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
00019  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
00020  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
00021  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
00022  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
00023  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
00024  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
00025  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  */
00028 /*
00029  *  RoboPeak LIDAR System
00030  *  RPlidar ROS Node client test app
00031  *
00032  *  Copyright 2009 - 2014 RoboPeak Team
00033  *  http://www.robopeak.com
00034  * 
00035  */
00036 
00037 
00038 #include "ros/ros.h"
00039 #include "sensor_msgs/LaserScan.h"
00040 
00041 #define RAD2DEG(x) ((x)*180./M_PI)
00042 
00043 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
00044 {
00045     int count = scan->scan_time / scan->time_increment;
00046     ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
00047     ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));
00048   
00049     for(int i = 0; i < count; i++) {
00050         float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
00051         ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
00052     }
00053 }
00054 
00055 int main(int argc, char **argv)
00056 {
00057     ros::init(argc, argv, "rplidar_node_client");
00058     ros::NodeHandle n;
00059 
00060     ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);
00061 
00062     ros::spin();
00063 
00064     return 0;
00065 }


rplidar_ros
Author(s):
autogenerated on Fri Dec 16 2016 03:59:16