LMS1xx_node.cpp
Go to the documentation of this file.
00001 #include <csignal>
00002 #include <cstdio>
00003 #include <LMS1xx.h>
00004 #include "ros/ros.h"
00005 #include "sensor_msgs/LaserScan.h"
00006 
00007 #define DEG2RAD M_PI/180.0
00008 
00009 int main(int argc, char **argv)
00010 {
00011   // laser data
00012   LMS1xx laser;
00013   scanCfg cfg;
00014   scanDataCfg dataCfg;
00015   scanData data;
00016   // published data
00017   sensor_msgs::LaserScan scan_msg;
00018   // parameters
00019   std::string host;
00020   std::string frame_id;
00021 
00022   ros::init(argc, argv, "lms1xx");
00023   ros::NodeHandle nh;
00024   ros::NodeHandle n("~");
00025   ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
00026 
00027   n.param<std::string>("host", host, "192.168.1.2");
00028   n.param<std::string>("frame_id", frame_id, "laser");
00029 
00030   ROS_INFO("connecting to laser at : %s", host.c_str());
00031   // initialize hardware
00032   laser.connect(host);
00033 
00034   if (laser.isConnected())
00035   {
00036     ROS_INFO("Connected to laser.");
00037 
00038     laser.login();
00039     cfg = laser.getScanCfg();
00040 
00041     scan_msg.header.frame_id = frame_id;
00042 
00043     scan_msg.range_min = 0.01;
00044     scan_msg.range_max = 20.0;
00045 
00046     scan_msg.scan_time = 100.0/cfg.scaningFrequency;
00047 
00048     scan_msg.angle_increment = (double)cfg.angleResolution/10000.0 * DEG2RAD;
00049     scan_msg.angle_min = (double)cfg.startAngle/10000.0 * DEG2RAD - M_PI/2;
00050     scan_msg.angle_max = (double)cfg.stopAngle/10000.0 * DEG2RAD - M_PI/2;
00051 
00052     std::cout << "resolution : " << (double)cfg.angleResolution/10000.0 << " deg " << std::endl;
00053     std::cout << "frequency : " << (double)cfg.scaningFrequency/100.0 << " Hz " << std::endl;
00054 
00055     int num_values;
00056     if (cfg.angleResolution == 2500)
00057     {
00058       num_values = 1081;
00059     }
00060     else if (cfg.angleResolution == 5000)
00061     {
00062       num_values = 541;
00063     }
00064     else
00065     {
00066       ROS_ERROR("Unsupported resolution");
00067       return 0;
00068     }
00069 
00070     scan_msg.time_increment = scan_msg.scan_time/num_values;
00071 
00072     scan_msg.ranges.resize(num_values);
00073     scan_msg.intensities.resize(num_values);
00074 
00075     dataCfg.outputChannel = 1;
00076     dataCfg.remission = true;
00077     dataCfg.resolution = 1;
00078     dataCfg.encoder = 0;
00079     dataCfg.position = false;
00080     dataCfg.deviceName = false;
00081     dataCfg.outputInterval = 1;
00082 
00083     laser.setScanDataCfg(dataCfg);
00084 
00085     laser.startMeas();
00086 
00087     status_t stat;
00088     do // wait for ready status
00089     {
00090       stat = laser.queryStatus();
00091       ros::Duration(1.0).sleep();
00092     }
00093     while (stat != ready_for_measurement);
00094 
00095     laser.startDevice(); // Log out to properly re-enable system after config
00096 
00097     laser.scanContinous(1);
00098 
00099     while (ros::ok())
00100     {
00101       ros::Time start = ros::Time::now();
00102 
00103       scan_msg.header.stamp = start;
00104       ++scan_msg.header.seq;
00105 
00106       laser.getData(data);
00107 
00108       for (int i = 0; i < data.dist_len1; i++)
00109       {
00110         scan_msg.ranges[i] = data.dist1[i] * 0.001;
00111       }
00112 
00113       for (int i = 0; i < data.rssi_len1; i++)
00114       {
00115         scan_msg.intensities[i] = data.rssi1[i];
00116       }
00117 
00118       scan_pub.publish(scan_msg);
00119 
00120       ros::spinOnce();
00121     }
00122 
00123     laser.scanContinous(0);
00124     laser.stopMeas();
00125     laser.disconnect();
00126   }
00127   else
00128   {
00129     ROS_ERROR("Connection to device failed");
00130   }
00131   return 0;
00132 }


lms1xx
Author(s): Konrad Banachowicz
autogenerated on Mon Oct 6 2014 04:03:00