urg_c_wrapper.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
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  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Chad Rockey, Mike O'Driscoll
32  */
33 
34 #include "ros/ros.h"
35 #include <urg_node/urg_c_wrapper.h>
36 #include <limits>
37 #include <string>
38 #include <vector>
39 #include <boost/crc.hpp>
40 
41 namespace urg_node
42 {
43 
44 URGCWrapper::URGCWrapper(const std::string& ip_address, const int ip_port,
45  bool& using_intensity, bool& using_multiecho, bool synchronize_time)
46 {
47  // Store for comprehensive diagnostics
48  ip_address_ = ip_address;
49  ip_port_ = ip_port;
50  serial_port_ = "";
51  serial_baud_ = 0;
52 
53 
54  long baudrate_or_port = (long)ip_port;
55  const char *device = ip_address.c_str();
56 
57  int result = urg_open(&urg_, URG_ETHERNET, device, baudrate_or_port);
58  if (result < 0)
59  {
60  std::stringstream ss;
61  ss << "Could not open network Hokuyo:\n";
62  ss << ip_address << ":" << ip_port << "\n";
63  ss << urg_error(&urg_);
64  throw std::runtime_error(ss.str());
65  }
66 
67  initialize(using_intensity, using_multiecho, synchronize_time);
68 }
69 
70 URGCWrapper::URGCWrapper(const int serial_baud, const std::string& serial_port,
71  bool& using_intensity, bool& using_multiecho, bool synchronize_time)
72 {
73  // Store for comprehensive diagnostics
74  serial_baud_ = serial_baud;
75  serial_port_ = serial_port;
76  ip_address_ = "";
77  ip_port_ = 0;
78 
79  long baudrate_or_port = (long)serial_baud;
80  const char *device = serial_port.c_str();
81 
82  int result = urg_open(&urg_, URG_SERIAL, device, baudrate_or_port);
83  if (result < 0)
84  {
85  std::stringstream ss;
86  ss << "Could not open serial Hokuyo:\n";
87  ss << serial_port << " @ " << serial_baud << "\n";
88  ss << urg_error(&urg_);
89  stop();
90  urg_close(&urg_);
91  throw std::runtime_error(ss.str());
92  }
93 
94  initialize(using_intensity, using_multiecho, synchronize_time);
95 }
96 
97 void URGCWrapper::initialize(bool& using_intensity, bool& using_multiecho, bool synchronize_time)
98 {
99  int urg_data_size = urg_max_data_size(&urg_);
100  // urg_max_data_size can return a negative, error code value. Resizing based on this value will fail.
101  if (urg_data_size < 0)
102  {
103  // This error can be caused by a URG-04LX in SCIP 1.1 mode, so we try to set SCIP 2.0 mode.
104  if (setToSCIP2() && urg_max_data_size(&urg_) >= 0)
105  {
106  // If setting SCIP 2.0 was successful, we set urg_data_size to the correct value.
107  urg_data_size = urg_max_data_size(&urg_);
108  }
109  else
110  {
111  urg_.last_errno = urg_data_size;
112  std::stringstream ss;
113  ss << "Could not initialize Hokuyo:\n";
114  ss << urg_error(&urg_);
115  stop();
116  urg_close(&urg_);
117  throw std::runtime_error(ss.str());
118  }
119  }
120  // Ocassionally urg_max_data_size returns a string pointer, make sure we don't allocate too much space,
121  // the current known max is 1440 steps
122  if (urg_data_size > 5000)
123  {
124  urg_data_size = 5000;
125  }
126  data_.resize(urg_data_size * URG_MAX_ECHO);
127  intensity_.resize(urg_data_size * URG_MAX_ECHO);
128 
129  started_ = false;
130  frame_id_ = "";
131  first_step_ = 0;
132  last_step_ = 0;
133  cluster_ = 1;
134  skip_ = 0;
135 
136  synchronize_time_ = synchronize_time;
137  hardware_clock_ = 0.0;
139  hardware_clock_adj_ = 0.0;
140  adj_count_ = 0;
141 
142  if (using_intensity)
143  {
144  using_intensity = isIntensitySupported();
145  }
146 
147  if (using_multiecho)
148  {
149  using_multiecho = isMultiEchoSupported();
150  }
151 
152  use_intensity_ = using_intensity;
153  use_multiecho_ = using_multiecho;
154 
157  {
159  }
160  else if (use_intensity_)
161  {
163  }
164  else if (use_multiecho_)
165  {
167  }
168 }
169 
171 {
172  if (!started_)
173  {
175  if (result < 0)
176  {
177  std::stringstream ss;
178  ss << "Could not start Hokuyo measurement:\n";
179  if (use_intensity_)
180  {
181  ss << "With Intensity" << "\n";
182  }
183  if (use_multiecho_)
184  {
185  ss << "With MultiEcho" << "\n";
186  }
187  ss << urg_error(&urg_);
188  throw std::runtime_error(ss.str());
189  }
190  }
191  started_ = true;
192 }
193 
195 {
197  started_ = false;
198 }
199 
201 {
202  stop();
203  urg_close(&urg_);
204 }
205 
206 bool URGCWrapper::grabScan(const sensor_msgs::LaserScanPtr& msg)
207 {
208  msg->header.frame_id = frame_id_;
209  msg->angle_min = getAngleMin();
210  msg->angle_max = getAngleMax();
211  msg->angle_increment = getAngleIncrement();
212  msg->scan_time = getScanPeriod();
213  msg->time_increment = getTimeIncrement();
214  msg->range_min = getRangeMin();
215  msg->range_max = getRangeMax();
216 
217  // Grab scan
218  int num_beams = 0;
219  long time_stamp = 0;
220  unsigned long long system_time_stamp = 0;
221 
222  if (use_intensity_)
223  {
224  num_beams = urg_get_distance_intensity(&urg_, &data_[0], &intensity_[0], &time_stamp, &system_time_stamp);
225  }
226  else
227  {
228  num_beams = urg_get_distance(&urg_, &data_[0], &time_stamp, &system_time_stamp);
229  }
230  if (num_beams <= 0)
231  {
232  return false;
233  }
234 
235  // Fill scan
236  if (synchronize_time_)
237  {
238  msg->header.stamp = getSynchronizedTime(time_stamp, system_time_stamp);
239  }
240  else
241  {
242  msg->header.stamp.fromNSec((uint64_t)system_time_stamp);
243  }
244  msg->header.stamp = msg->header.stamp + system_latency_ + user_latency_ + getAngularTimeOffset();
245  msg->ranges.resize(num_beams);
246  if (use_intensity_)
247  {
248  msg->intensities.resize(num_beams);
249  }
250 
251  for (int i = 0; i < num_beams; i++)
252  {
253  if (data_[(i) + 0] != 0)
254  {
255  msg->ranges[i] = static_cast<float>(data_[i]) / 1000.0;
256  if (use_intensity_)
257  {
258  msg->intensities[i] = intensity_[i];
259  }
260  }
261  else
262  {
263  msg->ranges[i] = std::numeric_limits<float>::quiet_NaN();
264  continue;
265  }
266  }
267  return true;
268 }
269 
270 bool URGCWrapper::grabScan(const sensor_msgs::MultiEchoLaserScanPtr& msg)
271 {
272  msg->header.frame_id = frame_id_;
273  msg->angle_min = getAngleMin();
274  msg->angle_max = getAngleMax();
275  msg->angle_increment = getAngleIncrement();
276  msg->scan_time = getScanPeriod();
277  msg->time_increment = getTimeIncrement();
278  msg->range_min = getRangeMin();
279  msg->range_max = getRangeMax();
280 
281  // Grab scan
282  int num_beams = 0;
283  long time_stamp = 0;
284  unsigned long long system_time_stamp;
285 
286  if (use_intensity_)
287  {
288  num_beams = urg_get_multiecho_intensity(&urg_, &data_[0], &intensity_[0], &time_stamp, &system_time_stamp);
289  }
290  else
291  {
292  num_beams = urg_get_multiecho(&urg_, &data_[0], &time_stamp, &system_time_stamp);
293  }
294  if (num_beams <= 0)
295  {
296  return false;
297  }
298 
299  // Fill scan (uses vector.reserve wherever possible to avoid initalization and unecessary memory expansion)
300  msg->header.stamp.fromNSec((uint64_t)system_time_stamp);
301  msg->header.stamp = msg->header.stamp + system_latency_ + user_latency_ + getAngularTimeOffset();
302  msg->ranges.reserve(num_beams);
303  if (use_intensity_)
304  {
305  msg->intensities.reserve(num_beams);
306  }
307 
308  for (size_t i = 0; i < num_beams; i++)
309  {
310  sensor_msgs::LaserEcho range_echo;
311  range_echo.echoes.reserve(URG_MAX_ECHO);
312  sensor_msgs::LaserEcho intensity_echo;
313  if (use_intensity_)
314  {
315  intensity_echo.echoes.reserve(URG_MAX_ECHO);
316  }
317  for (size_t j = 0; j < URG_MAX_ECHO; j++)
318  {
319  if (data_[(URG_MAX_ECHO * i) + j] != 0)
320  {
321  range_echo.echoes.push_back(static_cast<float>(data_[(URG_MAX_ECHO * i) + j]) / 1000.0f);
322  if (use_intensity_)
323  {
324  intensity_echo.echoes.push_back(intensity_[(URG_MAX_ECHO * i) + j]);
325  }
326  }
327  else
328  {
329  break;
330  }
331  }
332  msg->ranges.push_back(range_echo);
333  if (use_intensity_)
334  {
335  msg->intensities.push_back(intensity_echo);
336  }
337  }
338 
339  return true;
340 }
341 
343 {
344  // Construct and write AR00 command.
345  std::string str_cmd;
346  str_cmd += 0x02; // STX
347  str_cmd.append("000EAR00A012"); // AR00 cmd with length and checksum.
348  str_cmd += 0x03; // ETX
349 
350  // Get the response
351  std::string response = sendCommand(str_cmd);
352 
353  ROS_DEBUG_STREAM("Full response: " << response);
354 
355  // Strip STX and ETX before calculating the CRC.
356  response.erase(0, 1);
357  response.erase(response.size() - 1, 1);
358 
359  // Get the CRC, it's the last 4 chars.
360  std::stringstream ss;
361  ss << response.substr(response.size() - 4, 4);
362  uint16_t crc;
363  ss >> std::hex >> crc;
364 
365  // Remove the CRC from the check.
366  std::string msg = response.substr(0, response.size() - 4);
367  // Check the checksum.
368  uint16_t checksum_result = checkCRC(msg.data(), msg.size());
369 
370  if (checksum_result != crc)
371  {
372  ROS_WARN("Received bad frame, incorrect checksum");
373  return false;
374  }
375 
376  // Debug output reponse up to scan data.
377  ROS_DEBUG_STREAM("Response: " << response.substr(0, 41));
378  // Decode the result if crc checks out.
379  // Grab the status
380  ss.clear();
381  ROS_DEBUG_STREAM("Status " << response.substr(8, 2));
382  ss << response.substr(8, 2); // Status is 8th position 2 chars.
383  ss >> std::hex >> status.status;
384 
385  if (status.status != 0)
386  {
387  ROS_WARN("Received bad status");
388  return false;
389  }
390 
391  // Grab the operating mode
392  ss.clear();
393  ROS_DEBUG_STREAM("Operating mode " << response.substr(10, 1););
394  ss << response.substr(10, 1);
395  ss >> std::hex >> status.operating_mode;
396 
397  // Grab the area number
398  ss.clear();
399  ss << response.substr(11, 2);
400  ROS_DEBUG_STREAM("Area Number " << response.substr(11, 2));
401  ss >> std::hex >> status.area_number;
402  // Per documentation add 1 to offset area number
403  status.area_number++;
404 
405  // Grab the Error Status
406  ss.clear();
407  ss << response.substr(13, 1);
408  ROS_DEBUG_STREAM("Error status " << response.substr(13, 1));
409  ss >> std::hex >> status.error_status;
410 
411 
412  // Grab the error code
413  ss.clear();
414  ss << response.substr(14, 2);
415  ROS_DEBUG_STREAM("Error code " << std::hex << response.substr(14, 2));
416  ss >> std::hex >> status.error_code;
417  // Offset by 0x40 is non-zero as per documentation
418  if (status.error_code != 0)
419  {
420  status.error_code += 0x40;
421  }
422 
423  // Get the lockout status
424  ss.clear();
425  ss << response.substr(16, 1);
426  ROS_DEBUG_STREAM("Lockout " << response.substr(16, 1));
427  ss >> std::hex >> status.lockout_status;
428 
429  return true;
430 }
431 
433 {
434  // Construct and write DL00 command.
435  std::string str_cmd;
436  str_cmd += 0x02; // STX
437  str_cmd.append("000EDL005BCB"); // DL00 cmd with length and checksum.
438  str_cmd += 0x03; // ETX
439 
440  // Get the response
441  std::string response = sendCommand(str_cmd);
442 
443  ROS_DEBUG_STREAM("Full response: " << response);
444 
445  // Strip STX and ETX before calculating the CRC.
446  response.erase(0, 1);
447  response.erase(response.size() - 1, 1);
448 
449  // Get the CRC, it's the last 4 chars.
450  std::stringstream ss;
451  ss << response.substr(response.size() - 4, 4);
452  uint16_t crc;
453  ss >> std::hex >> crc;
454 
455  // Remove the CRC from the check.
456  std::string msg = response.substr(0, response.size() - 4);
457  // Check the checksum.
458  uint16_t checksum_result = checkCRC(msg.data(), msg.size());
459 
460  if (checksum_result != crc)
461  {
462  ROS_WARN("Received bad frame, incorrect checksum");
463  return false;
464  }
465 
466  // Decode the result if crc checks out.
467  // Grab the status
468  uint16_t status = 0;
469  ss.clear();
470  ROS_DEBUG_STREAM("Status " << response.substr(8, 2));
471  ss << response.substr(8, 2); // Status is 8th position 2 chars.
472  ss >> std::hex >> status;
473 
474  if (status != 0)
475  {
476  ROS_WARN("Received bad status");
477  return false;
478  }
479 
480  std::vector<UrgDetectionReport> reports;
481  msg = msg.substr(10); // remove the header.
482  // Process the message, there are 29 reports.
483  // The 30th report is a circular buff marker of area with 0xFF
484  // denoting the "last" report being the previous one.
485  for (int i = 0; i < 30; i++)
486  {
487  uint16_t area = 0;
488  uint16_t distance = 0;
489  uint16_t step = 0;
490  ss.clear();
491 
492  // Each msg is 64 chars long, offset which
493  // report is being read in.
494  uint16_t offset_pos = i * 64;
495  ss << msg.substr(offset_pos, 2); // Area is 2 chars long
496  ss >> std::hex >> area;
497 
498 
499  ss.clear();
500  ss << msg.substr(offset_pos + 4, 4); // Distance is offset 4 from beginning, 4 chars long.
501  ss >> std::hex >> distance;
502 
503  ss.clear();
504  ss << msg.substr(offset_pos + 8, 4); // "Step" is offset 8 from beginning 4 chars long.
505  ss >> std::hex >> step;
506  ROS_DEBUG_STREAM(i << " Area: " << area << " Distance: " << distance << " Step: " << step);
507 
509  r.area = area;
510  r.distance = distance;
511  // From read value to angle of report is a value/8.
512  r.angle = static_cast<float>(step) / 8.0;
513 
514  reports.push_back(r);
515  }
516 
517  for (auto iter = reports.begin(); iter != reports.end(); ++iter)
518  {
519  // Check if value retrieved for area is FF.
520  // if it is this is the last element lasers circular buffer.
521  if (iter->area == 0xFF)
522  {
523  // Try and read the previous item.
524  // if it's the beginning, then all reports
525  // are empty.
526  if (iter - 1 == reports.begin())
527  {
528  ROS_WARN("All reports are empty, no detections available.");
529  report.status = status;
530  return false;
531  }
532  if (iter - 1 != reports.begin())
533  {
534  report = *(iter-1);
535  report.area += 1; // Final area is offset by 1.
536  report.status = status;
537  break;
538  }
539  }
540  }
541 
542  return true;
543 }
544 
546 {
548  return false;
549 
550  char buffer[sizeof("SCIP2.0\n")];
551  int n;
552 
553  do
554  {
555  n = serial_readline(&(urg_.connection.serial), buffer, sizeof(buffer), 1000);
556  }
557  while (n >= 0);
558 
559  serial_write(&(urg_.connection.serial), "SCIP2.0\n", sizeof(buffer));
560  n = serial_readline(&(urg_.connection.serial), buffer, sizeof(buffer), 1000);
561 
562  // Check if switching was successful.
563  if (n > 0 && strcmp(buffer, "SCIP2.0") == 0
564  && urg_open(&urg_, URG_SERIAL, serial_port_.c_str(), (long)serial_baud_) >= 0)
565  {
566  ROS_DEBUG_STREAM("Set sensor to SCIP 2.0.");
567  return true;
568  }
569  return false;
570 }
571 
572 uint16_t URGCWrapper::checkCRC(const char* bytes, const uint32_t size)
573 {
574  boost::crc_optimal<16, 0x1021, 0, 0, true, true> crc_kermit_type;
575  crc_kermit_type.process_bytes(bytes, size);
576  return crc_kermit_type.checksum();
577 }
578 
579 std::string URGCWrapper::sendCommand(std::string cmd)
580 {
581  std::string result;
582  bool restart = false;
583 
584  if (isStarted())
585  {
586  restart = true;
587  // Scan must stop before sending a command
588  stop();
589  }
590 
591  // Get the socket reference and send
593  write(sock, cmd.c_str(), cmd.size());
594 
595  // All serial command structures start with STX + LEN as
596  // the first 5 bytes, read those in.
597  size_t total_read_len = 0;
598  size_t read_len = 0;
599  // Read in the header, make sure we get all 5 bytes expcted
600  char recvb[5] = {0};
601  ssize_t expected_read = 5;
602  while (total_read_len < expected_read)
603  {
604  read_len = read(sock, recvb + total_read_len, expected_read - total_read_len); // READ STX
605  total_read_len += read_len;
606  if (read_len <= 0)
607  {
608  ROS_ERROR("Read socket failed: %s", strerror(errno));
609  result.clear();
610  return result;
611  }
612  }
613 
614  std::string recv_header(recvb, read_len);
615  // Convert the read len from hex chars to int.
616  std::stringstream ss;
617  ss << recv_header.substr(1, 4);
618  ss >> std::hex >> expected_read;
619  ROS_DEBUG_STREAM("Read len " << expected_read);
620 
621  // Already read len of 5, take that out.
622  uint32_t arr_size = expected_read - 5;
623  // Bounds check the size, we really shouldn't exceed 8703 bytes
624  // based on the currently known messages on the hokuyo documentations
625  if (arr_size > 10000)
626  {
627  ROS_ERROR("Buffer creation bounds exceeded, shouldn't allocate: %u bytes", arr_size);
628  result.clear();
629  return result;
630  }
631 
632  ROS_DEBUG_STREAM("Creating buffer read of arr_Size: " << arr_size);
633  // Create buffer space for read.
635  data.reset(new char[arr_size]);
636 
637  // Read the remaining command
638  total_read_len = 0;
639  read_len = 0;
640  expected_read = arr_size;
641 
642  ROS_DEBUG_STREAM("Expected body size: " << expected_read);
643  while (total_read_len < expected_read)
644  {
645  read_len = read(sock, data.get()+total_read_len, expected_read - total_read_len);
646  total_read_len += read_len;
647  ROS_DEBUG_STREAM("Read in after header " << read_len);
648  if (read_len <= 0)
649  {
650  ROS_ERROR("Read socket failed: %s", strerror(errno));
651  result.clear();
652  return result;
653  }
654  }
655 
656  // Combine the read portions to return for processing.
657  result += recv_header;
658  result += std::string(data.get(), expected_read);
659 
660  // Resume scan after sending.
661  if (restart)
662  {
663  start();
664  }
665 
666  return result;
667 }
668 
670 {
671  return started_;
672 }
673 
675 {
676  long minr;
677  long maxr;
678  urg_distance_min_max(&urg_, &minr, &maxr);
679  return static_cast<double>(minr) / 1000.0;
680 }
681 
683 {
684  long minr;
685  long maxr;
686  urg_distance_min_max(&urg_, &minr, &maxr);
687  return static_cast<double>(maxr) / 1000.0;
688 }
689 
691 {
692  return urg_step2rad(&urg_, first_step_);
693 }
694 
696 {
697  return urg_step2rad(&urg_, last_step_);
698 }
699 
701 {
702  int min_step;
703  int max_step;
704  urg_step_min_max(&urg_, &min_step, &max_step);
705  return urg_step2rad(&urg_, min_step);
706 }
707 
709 {
710  int min_step;
711  int max_step;
712  urg_step_min_max(&urg_, &min_step, &max_step);
713  return urg_step2rad(&urg_, max_step);
714 }
715 
717 {
718  double angle_min = getAngleMin();
719  double angle_max = getAngleMax();
720  return cluster_ * (angle_max - angle_min) / static_cast<double>(last_step_ - first_step_);
721 }
722 
724 {
725  long scan_usec = urg_scan_usec(&urg_);
726  return 1.e-6 * static_cast<double>(scan_usec);
727 }
728 
730 {
731  int min_step;
732  int max_step;
733  urg_step_min_max(&urg_, &min_step, &max_step);
734  double scan_period = getScanPeriod();
735  double circle_fraction = (getAngleMaxLimit() - getAngleMinLimit()) / (2.0 * 3.141592);
736  return cluster_ * circle_fraction * scan_period / static_cast<double>(max_step - min_step);
737 }
738 
739 
740 std::string URGCWrapper::getIPAddress() const
741 {
742  return ip_address_;
743 }
744 
746 {
747  return ip_port_;
748 }
749 
750 std::string URGCWrapper::getSerialPort() const
751 {
752  return serial_port_;
753 }
754 
756 {
757  return serial_baud_;
758 }
759 
761 {
762  return std::string(urg_sensor_vendor(&urg_));
763 }
764 
766 {
767  return std::string(urg_sensor_product_type(&urg_));
768 }
769 
771 {
772  return std::string(urg_sensor_firmware_version(&urg_));
773 }
774 
776 {
777  return std::string(urg_sensor_firmware_date(&urg_));
778 }
779 
781 {
782  return std::string(urg_sensor_protocol_version(&urg_));
783 }
784 
786 {
787  return std::string(urg_sensor_serial_id(&urg_));
788 }
789 
791 {
792  return system_latency_;
793 }
794 
796 {
797  return user_latency_;
798 }
799 
801 {
802  return std::string(urg_sensor_status(&urg_));
803 }
804 
806 {
807  return std::string(urg_sensor_state(&urg_));
808 }
809 
810 void URGCWrapper::setFrameId(const std::string& frame_id)
811 {
812  frame_id_ = frame_id;
813 }
814 
815 void URGCWrapper::setUserLatency(const double latency)
816 {
817  user_latency_.fromSec(latency);
818 }
819 
820 // Must be called before urg_start
821 bool URGCWrapper::setAngleLimitsAndCluster(double& angle_min, double& angle_max, int cluster)
822 {
823  if (started_)
824  {
825  return false; // Must not be streaming
826  }
827 
828  // Set step limits
829  first_step_ = urg_rad2step(&urg_, angle_min);
830  last_step_ = urg_rad2step(&urg_, angle_max);
831  cluster_ = cluster;
832 
833  // Make sure step limits are not the same
834  if (first_step_ == last_step_)
835  {
836  // Make sure we're not at a limit
837  int min_step;
838  int max_step;
839  urg_step_min_max(&urg_, &min_step, &max_step);
840  if (first_step_ == min_step) // At beginning of range
841  {
842  last_step_ = first_step_ + 1;
843  }
844  else // At end of range (or all other cases)
845  {
846  first_step_ = last_step_ - 1;
847  }
848  }
849 
850  // Make sure angle_max is greater than angle_min (should check this after end limits)
851  if (last_step_ < first_step_)
852  {
853  double temp = first_step_;
855  last_step_ = temp;
856  }
857 
858  angle_min = urg_step2rad(&urg_, first_step_);
859  angle_max = urg_step2rad(&urg_, last_step_);
860  int result = urg_set_scanning_parameter(&urg_, first_step_, last_step_, cluster);
861  if (result < 0)
862  {
863  return false;
864  }
865  return true;
866 }
867 
868 void URGCWrapper::setSkip(int skip)
869 {
870  skip_ = skip;
871 }
872 
874 {
875  if (started_)
876  {
877  return false; // Must not be streaming
878  }
879 
881  int ret = urg_get_distance_intensity(&urg_, &data_[0], &intensity_[0], NULL, NULL);
882  if (ret <= 0)
883  {
884  return false; // Failed to start measurement with intensity: must not support it
885  }
887  return true;
888 }
889 
891 {
892  if (started_)
893  {
894  return false; // Must not be streaming
895  }
896 
898  int ret = urg_get_multiecho(&urg_, &data_[0], NULL, NULL);
899  if (ret <= 0)
900  {
901  return false; // Failed to start measurement with multiecho: must not support it
902  }
904  return true;
905 }
906 
908 {
909  // Adjust value for Hokuyo's timestamps
910  // Hokuyo's timestamps start from the rear center of the device (at Pi according to ROS standards)
911  double circle_fraction = 0.0;
912  if (first_step_ == 0 && last_step_ == 0)
913  {
914  circle_fraction = (getAngleMinLimit() + 3.141592) / (2.0 * 3.141592);
915  }
916  else
917  {
918  circle_fraction = (getAngleMin() + 3.141592) / (2.0 * 3.141592);
919  }
920  return ros::Duration(circle_fraction * getScanPeriod());
921 }
922 
924 {
926 
927  ros::Duration start_offset = getNativeClockOffset(1);
928  ros::Duration previous_offset;
929 
930  std::vector<ros::Duration> time_offsets(num_measurements);
931  for (size_t i = 0; i < num_measurements; i++)
932  {
933  ros::Duration scan_offset = getTimeStampOffset(1);
934  ros::Duration post_offset = getNativeClockOffset(1);
935  ros::Duration adjusted_scan_offset = scan_offset - start_offset;
936  ros::Duration adjusted_post_offset = post_offset - start_offset;
937  ros::Duration average_offset;
938  average_offset.fromSec((adjusted_post_offset.toSec() + previous_offset.toSec()) / 2.0);
939 
940  time_offsets[i] = adjusted_scan_offset - average_offset;
941 
942  previous_offset = adjusted_post_offset;
943  }
944 
945  // Get median value
946  // Sort vector using nth_element (partially sorts up to the median index)
947  std::nth_element(time_offsets.begin(), time_offsets.begin() + time_offsets.size() / 2, time_offsets.end());
948  system_latency_ = time_offsets[time_offsets.size() / 2];
949  // Angular time offset makes the output comparable to that of hokuyo_node
950  return system_latency_ + getAngularTimeOffset();
951 }
952 
954 {
955  if (started_)
956  {
957  std::stringstream ss;
958  ss << "Cannot get native clock offset while started.";
959  throw std::runtime_error(ss.str());
960  }
961 
963  {
964  std::stringstream ss;
965  ss << "Cannot start time stamp mode.";
966  throw std::runtime_error(ss.str());
967  }
968 
969  std::vector<ros::Duration> time_offsets(num_measurements);
970  for (size_t i = 0; i < num_measurements; i++)
971  {
972  ros::Time request_time = ros::Time::now();
973  ros::Time laser_time;
974  laser_time.fromNSec(1e6 * (uint64_t)urg_time_stamp(&urg_)); // 1e6 * milliseconds = nanoseconds
975  ros::Time response_time = ros::Time::now();
976  ros::Time average_time;
977  average_time.fromSec((response_time.toSec() + request_time.toSec()) / 2.0);
978  time_offsets[i] = laser_time - average_time;
979  }
980 
981  if (urg_stop_time_stamp_mode(&urg_) < 0)
982  {
983  std::stringstream ss;
984  ss << "Cannot stop time stamp mode.";
985  throw std::runtime_error(ss.str());
986  };
987 
988  // Return median value
989  // Sort vector using nth_element (partially sorts up to the median index)
990  std::nth_element(time_offsets.begin(), time_offsets.begin() + time_offsets.size() / 2, time_offsets.end());
991  return time_offsets[time_offsets.size() / 2];
992 }
993 
995 {
996  if (started_)
997  {
998  std::stringstream ss;
999  ss << "Cannot get time stamp offset while started.";
1000  throw std::runtime_error(ss.str());
1001  }
1002 
1003  start();
1004 
1005  std::vector<ros::Duration> time_offsets(num_measurements);
1006  for (size_t i = 0; i < num_measurements; i++)
1007  {
1008  long time_stamp;
1009  unsigned long long system_time_stamp;
1010  int ret = 0;
1011 
1013  {
1014  ret = urg_get_distance(&urg_, &data_[0], &time_stamp, &system_time_stamp);
1015  }
1017  {
1018  ret = urg_get_distance_intensity(&urg_, &data_[0], &intensity_[0], &time_stamp, &system_time_stamp);
1019  }
1020  else if (measurement_type_ == URG_MULTIECHO)
1021  {
1022  ret = urg_get_multiecho(&urg_, &data_[0], &time_stamp, &system_time_stamp);
1023  }
1025  {
1026  ret = urg_get_multiecho_intensity(&urg_, &data_[0], &intensity_[0], &time_stamp, &system_time_stamp);
1027  }
1028 
1029  if (ret <= 0)
1030  {
1031  std::stringstream ss;
1032  ss << "Cannot get scan to measure time stamp offset.";
1033  throw std::runtime_error(ss.str());
1034  }
1035 
1036  ros::Time laser_timestamp;
1037  laser_timestamp.fromNSec(1e6 * (uint64_t)time_stamp);
1038  ros::Time system_time;
1039  system_time.fromNSec((uint64_t)system_time_stamp);
1040 
1041  time_offsets[i] = laser_timestamp - system_time;
1042  }
1043 
1044  stop();
1045 
1046  // Return median value
1047  // Sort vector using nth_element (partially sorts up to the median index)
1048  std::nth_element(time_offsets.begin(), time_offsets.begin() + time_offsets.size() / 2, time_offsets.end());
1049  return time_offsets[time_offsets.size() / 2];
1050 }
1051 
1052 ros::Time URGCWrapper::getSynchronizedTime(long time_stamp, long long system_time_stamp)
1053 {
1054  ros::Time stamp, system_time;
1055  system_time.fromNSec((uint64_t)system_time_stamp);
1056  stamp = system_time;
1057 
1058  const uint32_t t1 = static_cast<uint32_t>(time_stamp);
1059  const uint32_t t0 = static_cast<uint32_t>(last_hardware_time_stamp_);
1060  // hokuyo uses a 24-bit counter, so mask out irrelevant bits
1061  const uint32_t mask = 0x00ffffff;
1062  double delta = static_cast<double>(mask&(t1-t0))/1000.0;
1063  hardware_clock_ += delta;
1064  double cur_adj = stamp.toSec() - hardware_clock_;
1065  if (adj_count_ > 0)
1066  {
1068  }
1069  else
1070  {
1071  // Initialize the EMA
1072  hardware_clock_adj_ = cur_adj;
1073  }
1074  adj_count_++;
1075  last_hardware_time_stamp_ = time_stamp;
1076 
1077  // Once hardware clock is synchronized, use it. Otherwise just return the
1078  // input system_time_stamp as ros::Time.
1079  if (adj_count_ > 100)
1080  {
1082  // If the time error is large a clock warp has occurred.
1083  // Reset the EMA and use the system time.
1084  if (fabs((stamp-system_time).toSec()) > 0.1)
1085  {
1086  adj_count_ = 0;
1087  hardware_clock_ = 0.0;
1089  stamp = system_time;
1090  ROS_INFO("%s: detected clock warp, reset EMA", __func__);
1091  }
1092  }
1093  return stamp;
1094 }
1095 } // namespace urg_node
int urg_start_time_stamp_mode(urg_t *urg)
msg
const char * urg_sensor_vendor(urg_t *urg)
std::string getIPAddress() const
int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp)
const double adj_alpha_
const char * urg_error(const urg_t *urg)
int urg_rad2step(const urg_t *urg, double radian)
int urg_stop_time_stamp_mode(urg_t *urg)
void urg_step_min_max(const urg_t *urg, int *min_index, int *max_index)
ros::Duration user_latency_
f
Time & fromNSec(uint64_t t)
int urg_max_data_size(const urg_t *urg)
urg_connection_t connection
std::string getDeviceID()
bool setAngleLimitsAndCluster(double &angle_min, double &angle_max, int cluster)
double getAngleMax() const
int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan)
std::string getProtocolVersion()
double getRangeMax() const
int serial_write(urg_serial_t *serial, const char *data, int size)
data
URGCWrapper(const std::string &ip_address, const int ip_port, bool &using_intensity, bool &using_multiecho, bool synchronize_time)
URG_ETHERNET
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
double getAngleMinLimit() const
urg_measurement_type_t measurement_type_
ros::Duration getTimeStampOffset(size_t num_measurements)
#define ROS_WARN(...)
std::string serial_port_
int urg_stop_measurement(urg_t *urg)
int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp)
Time & fromSec(double t)
uint16_t checkCRC(const char *bytes, const uint32_t size)
calculate the crc of a given set of bytes.
URG_MAX_ECHO
std::vector< unsigned short > intensity_
int last_errno
void initialize(bool &using_intensity, bool &using_multiecho, bool synchronize_time)
int getSerialBaud() const
long urg_time_stamp(urg_t *urg)
double getScanPeriod() const
std::string getVendorName()
ros::Duration getUserTimeOffset() const
int urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step)
const char * urg_sensor_firmware_date(urg_t *urg)
Duration & fromSec(double t)
#define ROS_INFO(...)
bool getDL00Status(UrgDetectionReport &report)
ros::Time getSynchronizedTime(long time_stamp, long long system_time_stamp)
Get synchronized time stamp using hardware clock.
void urg_close(urg_t *urg)
double getTimeIncrement() const
const char * urg_sensor_status(urg_t *urg)
int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port)
std::string getSensorStatus()
bool getAR00Status(URGStatus &status)
const char * urg_sensor_product_type(urg_t *urg)
int serial_readline(urg_serial_t *serial, char *data, int max_size, int timeout)
std::vector< long > data_
void urg_distance_min_max(const urg_t *urg, long *min_distance, long *max_distance)
const char * urg_sensor_firmware_version(urg_t *urg)
bool grabScan(const sensor_msgs::LaserScanPtr &msg)
#define ROS_DEBUG_STREAM(args)
URG_MULTIECHO_INTENSITY
std::string getFirmwareVersion()
std::string getSerialPort() const
double getRangeMin() const
int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp)
unsigned int step
urg_serial_t serial
URG_DISTANCE_INTENSITY
Duration & fromNSec(int64_t t)
ros::Duration getAngularTimeOffset() const
void setSkip(int skip)
double urg_step2rad(const urg_t *urg, int step)
int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp)
std::string getFirmwareDate()
static Time now()
ros::Duration system_latency_
double getAngleIncrement() const
std::string frame_id_
Output frame_id for each laserscan.
URG_DISTANCE
void setFrameId(const std::string &frame_id)
const char * urg_sensor_serial_id(urg_t *urg)
ros::Duration getComputedLatency() const
const char * urg_sensor_protocol_version(urg_t *urg)
ros::Duration getNativeClockOffset(size_t num_measurements)
#define ROS_ERROR(...)
urg_connection_type_t type
double getAngleMin() const
std::string getSensorState()
void setUserLatency(const double latency)
std::string getProductName()
bool setToSCIP2()
Set the Hokuyo URG-04LX from SCIP 1.1 mode to SCIP 2.0 mode.
URG_MULTIECHO
double getAngleMaxLimit() const
long urg_scan_usec(const urg_t *urg)
urg_tcpclient_t tcpclient
const char * urg_sensor_state(urg_t *urg)
URG_SERIAL
ros::Duration computeLatency(size_t num_measurements)
std::string sendCommand(std::string cmd)
Send an arbitrary serial command to the lidar. These commands can also be sent via the ethernet socke...


urg_node
Author(s): Chad Rockey , Mike O'Driscoll
autogenerated on Wed Oct 28 2020 03:39:16