36 #include "sensor_msgs/LaserScan.h"
38 #define DEG2RAD M_PI/180.0
40 int main(
int argc,
char **argv)
48 sensor_msgs::LaserScan scan_msg;
53 ros::init(argc, argv,
"lms1xx");
55 ros::NodeHandle n(
"~");
56 ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>(
"scan", 1);
58 n.param<std::string>(
"host", host,
"192.168.1.2");
59 n.param<std::string>(
"frame_id", frame_id,
"laser");
61 ROS_INFO(
"connecting to laser at : %s", host.c_str());
67 ROS_INFO(
"Connected to laser.");
72 scan_msg.header.frame_id = frame_id;
74 scan_msg.range_min = 0.01;
75 scan_msg.range_max = 40.0;
77 scan_msg.scan_time = 100.0/cfg.scaningFrequency;
79 scan_msg.angle_increment = (double)cfg.angleResolution/10000.0 *
DEG2RAD;
80 scan_msg.angle_min = (
double)cfg.startAngle/10000.0 *
DEG2RAD - M_PI/2;
81 scan_msg.angle_max = (double)cfg.stopAngle/10000.0 *
DEG2RAD - M_PI/2;
83 std::cout <<
"resolution : " << (
double)cfg.angleResolution/10000.0 <<
" deg " << std::endl;
84 std::cout <<
"frequency : " << (double)cfg.scaningFrequency/100.0 <<
" Hz " << std::endl;
87 if (cfg.angleResolution == 2500)
91 else if (cfg.angleResolution == 5000)
97 ROS_ERROR(
"Unsupported resolution");
101 scan_msg.time_increment = scan_msg.scan_time/num_values;
103 scan_msg.ranges.resize(num_values);
104 scan_msg.intensities.resize(num_values);
106 dataCfg.outputChannel = 1;
107 dataCfg.remission =
true;
108 dataCfg.resolution = 1;
110 dataCfg.position =
false;
111 dataCfg.deviceName =
false;
112 dataCfg.outputInterval = 1;
122 ros::Duration(1.0).sleep();
130 ros::Time start = ros::Time::now();
132 scan_msg.header.stamp = start;
133 ++scan_msg.header.seq;
137 for (
int i = 0; i < data.dist_len1; i++)
139 scan_msg.ranges[i] = data.dist1[i] * 0.001;
142 for (
int i = 0; i < data.rssi_len1; i++)
144 scan_msg.intensities[i] = data.rssi1[i];
147 scan_pub.publish(scan_msg);
158 ROS_ERROR(
"Connection to device failed");
void setScanDataCfg(const scanDataCfg &cfg)
Set scan data configuration. Set format of scan message returned by device.
scanCfg getScanCfg() const
Get current scan configuration. Get scan configuration :
status_t queryStatus()
Get current status of LMS1xx device.
int main(int argc, char **argv)
void login()
Log into LMS1xx unit. Increase privilege level, giving ability to change device configuration.
void stopMeas()
Stop measurements. After receiving this command LMS1xx unit stop spinning laser and measuring...
Structure containing single scan message.
Structure containing scan configuration.
void scanContinous(int start)
Start or stop continuous data acquisition. After reception of this command device start or stop conti...
void startMeas()
Start measurements. After receiving this command LMS1xx unit starts spinning laser and measuring...
bool isConnected()
Get status of connection.
void disconnect()
Disconnect from LMS1xx device.
void connect(std::string host, int port=2111)
Connect to LMS1xx.
void getData(scanData &data)
Receive single scan message.
Structure containing scan data configuration.
Class responsible for communicating with LMS1xx device.