Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
vpSickLDMRS.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Sick LD-MRS laser driver on UNIX platform.
33  *
34 *****************************************************************************/
35 
36 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
37 
38 #include <errno.h>
39 #include <fcntl.h>
40 #include <netdb.h>
41 #include <netinet/in.h>
42 #include <string.h>
43 #include <sys/select.h>
44 #include <sys/socket.h>
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpTime.h>
48 #include <visp3/sensor/vpSickLDMRS.h>
49 //#include <strings.h>
50 #include <assert.h>
51 #include <limits.h>
52 #include <math.h>
53 #include <stdlib.h>
54 
69  : socket_fd(-1), body(nullptr), vAngle(), time_offset(0), isFirstMeasure(true), maxlen_body(104000)
70 {
71  ip = "131.254.12.119";
72  port = 12002;
73  body = new unsigned char[maxlen_body];
74 
75  vAngle.resize(4); // Vertical angle of the 4 layers
76  vAngle[0] = vpMath::rad(-1.2);
77  vAngle[1] = vpMath::rad(-0.4);
78  vAngle[2] = vpMath::rad(0.4);
79  vAngle[3] = vpMath::rad(1.2);
80 }
81 
86 {
87  if (body)
88  delete[] body;
89 }
90 
100 bool vpSickLDMRS::setup(const std::string &ip_address, int com_port)
101 {
102  setIpAddress(ip_address);
103  setPort(com_port);
104  return (this->setup());
105 }
106 
113 {
114  struct sockaddr_in serv_addr;
115  int res;
116  struct timeval tv;
117  fd_set myset;
118 
119  // Create the TCP socket
120  socket_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
121  if (socket_fd < 0) {
122  fprintf(stderr, "Failed to create socket\n");
123  return false;
124  }
125  // bzero(&serv_addr, sizeof(serv_addr));
126  memset(&serv_addr, 0, sizeof(serv_addr));
127  serv_addr.sin_family = AF_INET; // Internet/IP
128  serv_addr.sin_addr.s_addr = inet_addr(ip.c_str()); // IP address
129  serv_addr.sin_port = htons(port); // server port
130 
131  // Establish connection
132  res = connect(socket_fd, (struct sockaddr *)&serv_addr, sizeof(serv_addr));
133  if (errno == EINPROGRESS) {
134  tv.tv_sec = 3;
135  tv.tv_usec = 0;
136  FD_ZERO(&myset);
137  FD_SET(static_cast<unsigned int>(socket_fd), &myset);
138  res = select(socket_fd + 1, nullptr, &myset, nullptr, &tv);
139  if (res < 0 && errno != EINTR) {
140  fprintf(stderr, "Error connecting to server %d - %s\n", errno, strerror(errno));
141  return false;
142  } else if (res > 0) {
143  fprintf(stderr, "ok");
144  } else {
145  fprintf(stderr, "Timeout in select() - Cancelling!\n");
146  return false;
147  }
148  }
149 
150  return true;
151 }
152 
160 {
161  unsigned int *uintptr;
162  unsigned short *ushortptr;
163  static unsigned char header[24];
164  ushortptr = (unsigned short *)header;
165  uintptr = (unsigned int *)header;
166 
167  assert(sizeof(header) == 24);
168  // std::cout << "size " << sizeof(header) << std::endl;
169 
170  double time_second = 0;
171 
172  if (isFirstMeasure) {
173  time_second = vpTime::measureTimeSecond();
174  }
175 
176  // read the 24 bytes header
177  if (recv(socket_fd, header, sizeof(header), MSG_WAITALL) == -1) {
178  printf("recv\n");
179  perror("recv");
180  return false;
181  }
182 
183  if (ntohl(uintptr[0]) != vpSickLDMRS::MagicWordC2) {
184  printf("Error, wrong magic number !!!\n");
185  return false;
186  }
187 
188  // get the message body
189  uint16_t msgtype = ntohs(ushortptr[7]);
190  uint32_t msgLength = ntohl(uintptr[2]);
191 
192  ssize_t len = recv(socket_fd, body, msgLength, MSG_WAITALL);
193  if (len != (ssize_t)msgLength) {
194  printf("Error, wrong msg length: %d of %d bytes.\n", (int)len, msgLength);
195  return false;
196  }
197 
198  if (msgtype != vpSickLDMRS::MeasuredData) {
199  // printf("The message in not relative to measured data !!!\n");
200  return true;
201  }
202 
203  // decode measured data
204 
205  // get the measurement number
206  unsigned short measurementId;
207  ushortptr = (unsigned short *)body;
208  measurementId = ushortptr[0];
209 
210  // get the start timestamp
211  uintptr = (unsigned int *)(body + 6);
212  unsigned int seconds = uintptr[1];
213  unsigned int fractional = uintptr[0];
214  double startTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32
215 
216  // get the end timestamp
217  uintptr = (unsigned int *)(body + 14);
218  seconds = uintptr[1];
219  fractional = uintptr[0];
220  double endTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32
221 
222  // compute the time offset to bring the measures in the Unix time reference
223  if (isFirstMeasure) {
224  time_offset = time_second - startTimestamp;
225  isFirstMeasure = false;
226  }
227 
228  startTimestamp += time_offset;
229  endTimestamp += time_offset;
230 
231  // get the number of steps per scanner rotation
232  unsigned short numSteps = ushortptr[11];
233 
234  // get the start/stop angle
235  short startAngle = (short)ushortptr[12];
236  short stopAngle = (short)ushortptr[13];
237 
238  // get the number of points of this measurement
239  unsigned short numPoints = ushortptr[14];
240 
241  int nlayers = 4;
242  for (int i = 0; i < nlayers; i++) {
243  laserscan[i].clear();
244  laserscan[i].setMeasurementId(measurementId);
245  laserscan[i].setStartTimestamp(startTimestamp);
246  laserscan[i].setEndTimestamp(endTimestamp);
247  laserscan[i].setNumSteps(numSteps);
248  laserscan[i].setStartAngle(startAngle);
249  laserscan[i].setStopAngle(stopAngle);
250  laserscan[i].setNumPoints(numPoints);
251  }
252 
253  // decode the measured points
254  double hAngle; // horizontal angle in rad
255  double rDist; // radial distance in meters
256  vpScanPoint scanPoint;
257 
258  if (numPoints > USHRT_MAX - 2)
259  throw(vpException(vpException::ioError, "Out of range number of point"));
260 
261  for (int i = 0; i < numPoints; i++) {
262  ushortptr = (unsigned short *)(body + 44 + i * 10);
263  unsigned char layer = ((unsigned char)body[44 + i * 10]) & 0x0F;
264  unsigned char echo = ((unsigned char)body[44 + i * 10]) >> 4;
265  // unsigned char flags = (unsigned char) body[44+i*10+1];
266 
267  if (echo == 0) {
268  hAngle = (2.f * M_PI / numSteps) * (short)ushortptr[1];
269  rDist = 0.01 * ushortptr[2]; // cm to meters conversion
270 
271  // vpTRACE("layer: %d d: %f hangle: %f", layer, rDist, hAngle);
272  scanPoint.setPolar(rDist, hAngle, vAngle[layer]);
273  laserscan[layer].addPoint(scanPoint);
274  }
275  }
276  return true;
277 }
278 
279 #endif
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ ioError
I/O error.
Definition: vpException.h:79
Implements a laser scan data structure that contains especially the list of scanned points that have ...
Definition: vpLaserScan.h:62
void setNumPoints(const unsigned short &num_points)
Definition: vpLaserScan.h:109
void setStopAngle(const short &stop_angle)
Definition: vpLaserScan.h:107
void setMeasurementId(const unsigned short &id)
Definition: vpLaserScan.h:97
void setNumSteps(const unsigned short &num_steps)
Definition: vpLaserScan.h:103
void setStartTimestamp(const double &start_timestamp)
Definition: vpLaserScan.h:99
void addPoint(const vpScanPoint &p)
Definition: vpLaserScan.h:86
void clear()
Definition: vpLaserScan.h:88
void setEndTimestamp(const double &end_timestamp)
Definition: vpLaserScan.h:101
void setStartAngle(const short &start_angle)
Definition: vpLaserScan.h:105
void setPort(int com_port)
std::string ip
void setIpAddress(std::string ip_address)
static double rad(double deg)
Definition: vpMath.h:127
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:73
void setPolar(double r_dist, double h_angle, double v_angle)
Definition: vpScanPoint.h:104
bool isFirstMeasure
Definition: vpSickLDMRS.h:156
virtual ~vpSickLDMRS() vp_override
Definition: vpSickLDMRS.cpp:85
unsigned char * body
Definition: vpSickLDMRS.h:153
size_t maxlen_body
Definition: vpSickLDMRS.h:157
vpColVector vAngle
Definition: vpSickLDMRS.h:154
double time_offset
Definition: vpSickLDMRS.h:155
bool measure(vpLaserScan laserscan[4])
VISP_EXPORT double measureTimeSecond()