43 #include "visp/vpSickLDMRS.h"
44 #include "visp/vpMath.h"
45 #include "visp/vpDebug.h"
46 #include "visp/vpTime.h"
47 #include <sys/socket.h>
48 #include <netinet/in.h>
75 ip =
"131.254.12.119";
77 body =
new unsigned char [104000];
78 isFirstMeasure =
true;
111 return ( this->
setup() );
121 struct sockaddr_in serv_addr;
127 socket_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
129 fprintf(stderr,
"Failed to create socket\n");
132 bzero(&serv_addr,
sizeof(serv_addr));
133 serv_addr.sin_family = AF_INET;
134 serv_addr.sin_addr.s_addr = inet_addr(
ip.c_str());
135 serv_addr.sin_port = htons(
port);
138 res = connect(
socket_fd, (
struct sockaddr *)&serv_addr,
sizeof(serv_addr)) ;
139 if (errno == EINPROGRESS) {
143 FD_SET(static_cast<unsigned int>(
socket_fd), &myset);
144 res = select(
socket_fd+1, NULL, &myset, NULL, &tv);
145 if (res < 0 && errno != EINTR) {
146 fprintf(stderr,
"Error connecting to server %d - %s\n", errno, strerror(errno));
150 fprintf(stderr,
"ok");
153 fprintf(stderr,
"Timeout in select() - Cancelling!\n");
169 unsigned int *uintptr;
170 unsigned short *ushortptr;
171 static unsigned char header[24];
172 ushortptr=(
unsigned short *)header;
173 uintptr=(
unsigned int *)header;
175 assert (
sizeof(header) == 24);
178 double time_second = 0;
180 if (isFirstMeasure) {
185 if (recv(
socket_fd, header,
sizeof(header), MSG_WAITALL) == -1) {
192 printf(
"Error, wrong magic number !!!\n");
197 uint16_t msgtype = ntohs(ushortptr[7]);
198 uint32_t msgLength = ntohl(uintptr[2]);
200 ssize_t len = recv(
socket_fd, body, msgLength, MSG_WAITALL);
201 if (len != (ssize_t)msgLength){
202 printf(
"Error, wrong msg length: %d of %d bytes.\n", (
int)len, msgLength);
214 unsigned short measurementId;
215 ushortptr = (
unsigned short *) body;
216 measurementId = ushortptr[0];
219 uintptr=(
unsigned int *) (body+6);
220 unsigned int seconds = uintptr[1];
221 unsigned int fractional=uintptr[0];
222 double startTimestamp = seconds + fractional / 4294967296.;
225 uintptr=(
unsigned int *) (body+14);
226 seconds = uintptr[1];
227 fractional=uintptr[0];
228 double endTimestamp = seconds + fractional / 4294967296.;
231 if (isFirstMeasure) {
232 time_offset = time_second - startTimestamp;
233 isFirstMeasure =
false;
236 startTimestamp += time_offset;
237 endTimestamp += time_offset;
240 unsigned short numSteps = ushortptr[11];
243 short startAngle = (short)ushortptr[12];
244 short stopAngle = (short)ushortptr[13];
249 unsigned short numPoints = ushortptr[14];
252 for (
int i=0; i < nlayers; i++) {
253 laserscan[i].
clear();
268 for (
int i=0; i < numPoints; i++) {
269 ushortptr = (
unsigned short *) (body+44+i*10);
270 unsigned char layer = ((
unsigned char) body[44+i*10])&0x0F;
271 unsigned char echo = ((
unsigned char) body[44+i*10])>>4;
275 hAngle = (2.f * M_PI / numSteps)*(
short) ushortptr[1];
276 rDist = 0.01 * ushortptr[2];
279 scanPoint.
setPolar(rDist, hAngle, vAngle[layer]);
280 laserscan[layer].
addPoint(scanPoint);
void setMeasurementId(const unsigned short &measurementId)
The magic word that allows to identify the messages that are sent by the Sick LD-MRS.
void setNumPoints(const unsigned short &numPoints)
void setStartAngle(const short &startAngle)
Implements a laser scan data structure that contains especially the list of scanned points that have ...
void setEndTimestamp(const double &endTimestamp)
void setIpAddress(std::string ip)
Class that defines a single laser scanner point.
Flag to indicate that the body of a message contains measured data.
static double rad(double deg)
void setStopAngle(const short &stopAngle)
bool measure(vpLaserScan laserscan[4])
void setNumSteps(const unsigned short &numSteps)
void setPolar(double rDist, double hAngle, double vAngle)
void addPoint(const vpScanPoint &p)
void setStartTimestamp(const double &startTimestamp)
static double measureTimeSecond()
void resize(const unsigned int i, const bool flagNullify=true)