35 #include <sensor_msgs/LaserScan.h> 36 #include <boost/asio.hpp> 38 #include <std_msgs/UInt16.h> 43 : port_(port), baud_rate_(baud_rate), shutting_down_(false), serial_(io, port_)
47 boost::asio::write(
serial_, boost::asio::buffer(
"b", 1));
52 boost::asio::write(
serial_, boost::asio::buffer(
"e", 1));
53 boost::asio::write(
serial_, boost::asio::buffer(
"e", 1));
59 uint8_t start_count = 0;
60 bool got_scan =
false;
61 boost::array<uint8_t, 1980> raw_bytes;
62 uint8_t good_sets = 0;
63 uint32_t motor_speed = 0;
70 boost::asio::read(
serial_, boost::asio::buffer(&raw_bytes[start_count],1));
74 if(raw_bytes[start_count] == 0xFA)
79 else if(start_count == 1)
81 if(raw_bytes[start_count] == 0xA0)
88 boost::asio::read(
serial_,boost::asio::buffer(&raw_bytes[2], 1978));
90 scan->angle_min = 0.0;
91 scan->angle_max = 2.0*M_PI-scan->angle_increment;
92 scan->angle_increment = (2.0*M_PI/360.0);
93 scan->range_min = 0.12;
94 scan->range_max = 5.0;
95 scan->ranges.resize(360);
96 scan->intensities.resize(360);
99 for(uint16_t i = 0; i < raw_bytes.size(); i=i+22)
101 if(raw_bytes[i] == 0xFA && raw_bytes[i+1] == (0xA0 + i / 22))
104 motor_speed += (raw_bytes[i+3] << 8) + raw_bytes[i+2];
105 rpms=(raw_bytes[i+3]<<8|raw_bytes[i+2])/10;
107 for(uint16_t j = i+4; j < i+20; j=j+4)
109 index = 4*(i/22) + (j-4-i)/4;
112 uint8_t byte0 = raw_bytes[j];
113 uint8_t byte1 = raw_bytes[j+1];
114 uint8_t byte2 = raw_bytes[j+2];
115 uint8_t byte3 = raw_bytes[j+3];
118 uint16_t range = ((byte1 & 0x3F)<< 8) + byte0;
121 uint16_t intensity = (byte3 << 8) + byte2;
123 scan->ranges[359-index] = range / 1000.0;
124 scan->intensities[359-index] = intensity;
129 scan->time_increment = motor_speed/good_sets/1e8;
140 int main(
int argc,
char **argv)
142 ros::init(argc, argv,
"hlds_lds2_publisher");
148 std::string frame_id;
150 std_msgs::UInt16
rpms;
152 priv_nh.
param(
"port", port, std::string(
"/dev/ttyUSB0"));
153 priv_nh.
param(
"baud_rate", baud_rate, 115200);
154 priv_nh.
param(
"frame_id", frame_id, std::string(
"laser"));
156 boost::asio::io_service io;
166 sensor_msgs::LaserScan::Ptr scan(
new sensor_msgs::LaserScan);
167 scan->header.frame_id = frame_id;
170 rpms.data=laser.
rpms;
172 motor_pub.publish(rpms);
178 catch (boost::system::system_error ex)
180 ROS_ERROR(
"An exception was thrown: %s", ex.what());
boost::asio::serial_port serial_
Actual serial port object for reading/writing to the LFCD Laser Scanner.
void publish(const boost::shared_ptr< M > &message) const
LFCDLaser(const std::string &port, uint32_t baud_rate, boost::asio::io_service &io)
Constructs a new LFCDLaser attached to the given serial port.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void close()
Close the driver down and prevent the polling loop from advancing.
~LFCDLaser()
Default destructor.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool shutting_down_
Flag for whether the driver is supposed to be shutting down or not.
uint16_t rpms
RPMS derived from the rpm bytes in an LFCD packet.
int main(int argc, char **argv)
uint32_t baud_rate_
The baud rate for the serial connection.
void poll(sensor_msgs::LaserScan::Ptr scan)
Poll the laser to get a new scan. Blocks until a complete new scan is received or close is called...