Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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(NULL), 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
100bool 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, NULL, &myset, NULL, &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)
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)
void setStopAngle(const short &stop_angle)
void setMeasurementId(const unsigned short &id)
Definition vpLaserScan.h:99
void setNumSteps(const unsigned short &num_steps)
void setStartTimestamp(const double &start_timestamp)
void addPoint(const vpScanPoint &p)
Definition vpLaserScan.h:87
void clear()
Definition vpLaserScan.h:89
void setEndTimestamp(const double &end_timestamp)
void setStartAngle(const short &start_angle)
void setPort(int com_port)
std::string ip
void setIpAddress(std::string ip_address)
static double rad(double deg)
Definition vpMath.h:116
Class that defines a single laser scanner point.
Definition vpScanPoint.h:73
void setPolar(double r_dist, double h_angle, double v_angle)
bool isFirstMeasure
unsigned char * body
size_t maxlen_body
vpColVector vAngle
double time_offset
virtual ~vpSickLDMRS()
bool measure(vpLaserScan laserscan[4])
VISP_EXPORT double measureTimeSecond()