36 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
41 #include <netinet/in.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>
69 : socket_fd(-1), body(nullptr), vAngle(), time_offset(0), isFirstMeasure(true), maxlen_body(104000)
71 ip =
"131.254.12.119";
104 return (this->
setup());
114 struct sockaddr_in serv_addr;
120 socket_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
122 fprintf(stderr,
"Failed to create socket\n");
126 memset(&serv_addr, 0,
sizeof(serv_addr));
127 serv_addr.sin_family = AF_INET;
128 serv_addr.sin_addr.s_addr = inet_addr(
ip.c_str());
129 serv_addr.sin_port = htons(
port);
132 res = connect(
socket_fd, (
struct sockaddr *)&serv_addr,
sizeof(serv_addr));
133 if (errno == EINPROGRESS) {
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));
142 }
else if (res > 0) {
143 fprintf(stderr,
"ok");
145 fprintf(stderr,
"Timeout in select() - Cancelling!\n");
161 unsigned int *uintptr;
162 unsigned short *ushortptr;
163 static unsigned char header[24];
164 ushortptr = (
unsigned short *)header;
165 uintptr = (
unsigned int *)header;
167 assert(
sizeof(header) == 24);
170 double time_second = 0;
177 if (recv(
socket_fd, header,
sizeof(header), MSG_WAITALL) == -1) {
184 printf(
"Error, wrong magic number !!!\n");
189 uint16_t msgtype = ntohs(ushortptr[7]);
190 uint32_t msgLength = ntohl(uintptr[2]);
193 if (len != (ssize_t)msgLength) {
194 printf(
"Error, wrong msg length: %d of %d bytes.\n", (
int)len, msgLength);
206 unsigned short measurementId;
207 ushortptr = (
unsigned short *)
body;
208 measurementId = ushortptr[0];
211 uintptr = (
unsigned int *)(
body + 6);
212 unsigned int seconds = uintptr[1];
213 unsigned int fractional = uintptr[0];
214 double startTimestamp = seconds + fractional / 4294967296.;
217 uintptr = (
unsigned int *)(
body + 14);
218 seconds = uintptr[1];
219 fractional = uintptr[0];
220 double endTimestamp = seconds + fractional / 4294967296.;
232 unsigned short numSteps = ushortptr[11];
235 short startAngle = (short)ushortptr[12];
236 short stopAngle = (short)ushortptr[13];
239 unsigned short numPoints = ushortptr[14];
242 for (
int i = 0; i < nlayers; i++) {
243 laserscan[i].
clear();
258 if (numPoints > USHRT_MAX - 2)
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;
268 hAngle = (2.f * M_PI / numSteps) * (
short)ushortptr[1];
269 rDist = 0.01 * ushortptr[2];
273 laserscan[layer].
addPoint(scanPoint);
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Implements a laser scan data structure that contains especially the list of scanned points that have ...
void setNumPoints(const unsigned short &num_points)
void setStopAngle(const short &stop_angle)
void setMeasurementId(const unsigned short &id)
void setNumSteps(const unsigned short &num_steps)
void setStartTimestamp(const double &start_timestamp)
void addPoint(const vpScanPoint &p)
void setEndTimestamp(const double &end_timestamp)
void setStartAngle(const short &start_angle)
void setPort(int com_port)
void setIpAddress(std::string ip_address)
static double rad(double deg)
Class that defines a single laser scanner point.
void setPolar(double r_dist, double h_angle, double v_angle)
virtual ~vpSickLDMRS() override
bool measure(vpLaserScan laserscan[4])
VISP_EXPORT double measureTimeSecond()