client.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014, RoboPeak
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  *
11  * 2. Redistributions in binary form must reproduce the above copyright notice,
12  * this list of conditions and the following disclaimer in the documentation
13  * and/or other materials provided with the distribution.
14  *
15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
17  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
18  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
19  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
20  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
21  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
22  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
23  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
24  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
25  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27  */
28 /*
29  * RoboPeak LIDAR System
30  * RPlidar ROS Node client test app
31  *
32  * Copyright 2009 - 2014 RoboPeak Team
33  * http://www.robopeak.com
34  *
35  */
36 
37 
38 #include "ros/ros.h"
39 #include "sensor_msgs/LaserScan.h"
40 
41 #define RAD2DEG(x) ((x)*180./M_PI)
42 
43 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
44 {
45  int count = scan->scan_time / scan->time_increment;
46  ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
47  ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));
48 
49  for(int i = 0; i < count; i++) {
50  float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
51  ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
52  }
53 }
54 
55 int main(int argc, char **argv)
56 {
57  ros::init(argc, argv, "rplidar_node_client");
59 
60  ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);
61 
62  ros::spin();
63 
64  return 0;
65 }
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan)
Definition: client.cpp:43
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
Definition: client.cpp:55
#define RAD2DEG(x)
Definition: client.cpp:41
#define ROS_INFO(...)
ROSCPP_DECL void spin()


rplidar_ros
Author(s):
autogenerated on Wed Jan 1 2020 04:01:40