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