lslidar_n301_driver.cc
Go to the documentation of this file.
1 /*
2  * This file is part of lslidar_n301 driver.
3  *
4  * The driver is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * The driver is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with the driver. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <string>
19 #include <cmath>
20 #include <unistd.h>
21 #include <sys/socket.h>
22 #include <arpa/inet.h>
23 #include <poll.h>
24 #include <errno.h>
25 #include <fcntl.h>
26 #include <sys/file.h>
27 
28 #include <ros/ros.h>
29 #include <tf/transform_listener.h>
30 
32 
33 namespace lslidar_n301_driver {
34 
37  nh(n),
38  pnh(pn),
39  socket_id(-1){
40  return;
41 }
42 
44  (void) close(socket_id);
45  return;
46 }
47 
49 
50  pnh.param("frame_id", frame_id, std::string("lslidar"));
51  pnh.param("device_ip", device_ip_string, std::string("192.168.1.222"));
52  pnh.param<int>("device_port", UDP_PORT_NUMBER, 2368);
53  inet_aton(device_ip_string.c_str(), &device_ip);
54  ROS_INFO_STREAM("Opening UDP socket: address " << device_ip_string);
55  ROS_INFO_STREAM("Opening UDP socket: port " << UDP_PORT_NUMBER);
56  return true;
57 }
58 
60 
61  // ROS diagnostics
62  diagnostics.setHardwareID("Lslidar_N301");
63  // n301 publishs 20*16 thousands points per second.
64  // Each packet contains 12 blocks. And each block
65  // contains 32 points. Together provides the
66  // packet rate.
67  const double diag_freq = 16*20000.0 / (12*32);
68  diag_max_freq = diag_freq;
69  diag_min_freq = diag_freq;
70  ROS_INFO("expected frequency: %.3f (Hz)", diag_freq);
71 
72  using namespace diagnostic_updater;
73  diag_topic.reset(new TopicDiagnostic(
74  "lslidar_packets", diagnostics,
77 
78  // Output
79  packet_pub = nh.advertise<lslidar_n301_msgs::LslidarN301Packet>(
80  "lslidar_packet", 100);
81 
82  return true;
83 }
84 
86  socket_id = socket(PF_INET, SOCK_DGRAM, 0);
87  if (socket_id == -1) {
88  perror("socket");
89  return false;
90  }
91 
92  sockaddr_in my_addr; // my address information
93  memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
94  my_addr.sin_family = AF_INET; // host byte order
95  my_addr.sin_port = htons(UDP_PORT_NUMBER); // short, in network byte order
96  ROS_INFO_STREAM("Opening UDP socket: port " << UDP_PORT_NUMBER);
97  my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
98 
99  if (bind(socket_id, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1) {
100  perror("bind"); // TODO: ROS_ERROR errno
101  return false;
102  }
103 
104  if (fcntl(socket_id, F_SETFL, O_NONBLOCK|FASYNC) < 0) {
105  perror("non-block");
106  return false;
107  }
108 
109  return true;
110 }
111 
113  if (!loadParameters()) {
114  ROS_ERROR("Cannot load all required ROS parameters...");
115  return false;
116  }
117 
118  if (!createRosIO()) {
119  ROS_ERROR("Cannot create all ROS IO...");
120  return false;
121  }
122 
123  if (!openUDPPort()) {
124  ROS_ERROR("Cannot open UDP port...");
125  return false;
126  }
127  ROS_INFO("Initialised lslidar n301 without error");
128  return true;
129 }
130 
132  lslidar_n301_msgs::LslidarN301PacketPtr& packet) {
133 
134  double time1 = ros::Time::now().toSec();
135 
136  struct pollfd fds[1];
137  fds[0].fd = socket_id;
138  fds[0].events = POLLIN;
139  static const int POLL_TIMEOUT = 2000; // one second (in msec)
140 
141  sockaddr_in sender_address;
142  socklen_t sender_address_len = sizeof(sender_address);
143 
144  while (true)
145  {
146  // Unfortunately, the Linux kernel recvfrom() implementation
147  // uses a non-interruptible sleep() when waiting for data,
148  // which would cause this method to hang if the device is not
149  // providing data. We poll() the device first to make sure
150  // the recvfrom() will not block.
151  //
152  // Note, however, that there is a known Linux kernel bug:
153  //
154  // Under Linux, select() may report a socket file descriptor
155  // as "ready for reading", while nevertheless a subsequent
156  // read blocks. This could for example happen when data has
157  // arrived but upon examination has wrong checksum and is
158  // discarded. There may be other circumstances in which a
159  // file descriptor is spuriously reported as ready. Thus it
160  // may be safer to use O_NONBLOCK on sockets that should not
161  // block.
162 
163  // poll() until input available
164  do {
165  int retval = poll(fds, 1, POLL_TIMEOUT);
166  if (retval < 0) // poll() error?
167  {
168  if (errno != EINTR)
169  ROS_ERROR("poll() error: %s", strerror(errno));
170  return 1;
171  }
172  if (retval == 0) // poll() timeout?
173  {
174  ROS_WARN("lslidar poll() timeout");
175  return 1;
176  }
177  if ((fds[0].revents & POLLERR)
178  || (fds[0].revents & POLLHUP)
179  || (fds[0].revents & POLLNVAL)) // device error?
180  {
181  ROS_ERROR("poll() reports lslidar error");
182  return 1;
183  }
184  } while ((fds[0].revents & POLLIN) == 0);
185 
186  // Receive packets that should now be available from the
187  // socket using a blocking read.
188  ssize_t nbytes = recvfrom(socket_id, &packet->data[0], PACKET_SIZE, 0,
189  (sockaddr*) &sender_address, &sender_address_len);
190 
191 // ROS_DEBUG_STREAM("incomplete lslidar packet read: "
192 // << nbytes << " bytes");
193 
194  if (nbytes < 0)
195  {
196  if (errno != EWOULDBLOCK)
197  {
198  perror("recvfail");
199  ROS_INFO("recvfail");
200  return 1;
201  }
202  }
203  else if ((size_t) nbytes == PACKET_SIZE)
204  {
205  // read successful,
206  // if packet is not from the lidar scanner we selected by IP,
207  // continue otherwise we are done
208  if( device_ip_string != "" && sender_address.sin_addr.s_addr != device_ip.s_addr )
209  continue;
210  else
211  break; //done
212  }
213 
214 
215 
216  }
217 
218 
219  // Average the times at which we begin and end reading. Use that to
220  // estimate when the scan occurred.
221  double time2 = ros::Time::now().toSec();
222  packet->stamp = ros::Time((time2 + time1) / 2.0);
223 
224  return 0;
225 }
226 
228 {
229  // Allocate a new shared pointer for zero-copy sharing with other nodelets.
230  lslidar_n301_msgs::LslidarN301PacketPtr packet(
231  new lslidar_n301_msgs::LslidarN301Packet());
232 
233  // Since the lslidar delivers data at a very high rate, keep
234  // reading and publishing scans as fast as possible.
235  //for (int i = 0; i < config_.npackets; ++i)
236  // {
237  // while (true)
238  // {
239  // // keep reading until full packet received
240  // int rc = input_->getPacket(&scan->packets[i]);
241  // if (rc == 0) break; // got a full packet?
242  // if (rc < 0) return false; // end of file reached?
243  // }
244  // }
245  while (true)
246  {
247  // keep reading until full packet received
248  int rc = getPacket(packet);
249  if (rc == 0) break; // got a full packet?
250  if (rc < 0) return false; // end of file reached?
251  }
252 
253  // publish message using time of last packet read
254  ROS_DEBUG("Publishing a full lslidar scan.");
255  packet_pub.publish(*packet);
256 
257  // notify diagnostics that a message has been published, updating
258  // its status
259  diag_topic->tick(packet->stamp);
261 
262  return true;
263 }
264 
265 } // namespace lslidar_driver
int getPacket(lslidar_n301_msgs::LslidarN301PacketPtr &msg)
void publish(const boost::shared_ptr< M > &message) const
void setHardwareID(const std::string &hwid)
#define ROS_WARN(...)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
static uint16_t PACKET_SIZE
diagnostic_updater::Updater diagnostics
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_INFO_STREAM(args)
static Time now()
boost::shared_ptr< diagnostic_updater::TopicDiagnostic > diag_topic
#define ROS_ERROR(...)
LslidarN301Driver(ros::NodeHandle &n, ros::NodeHandle &pn)
#define ROS_DEBUG(...)


lslidar_n301_driver
Author(s): Nick Shu
autogenerated on Thu Sep 26 2019 03:58:32