43 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
48 #include <netinet/in.h>
50 #include <sys/select.h>
51 #include <sys/socket.h>
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/core/vpTime.h>
55 #include <visp3/sensor/vpSickLDMRS.h>
69 vpSickLDMRS::vpSickLDMRS()
70 : socket_fd(-1), body(nullptr), vAngle(), time_offset(0), isFirstMeasure(true), maxlen_body(104000)
72 ip =
"131.254.12.119";
74 body =
new unsigned char[maxlen_body];
86 vpSickLDMRS::~vpSickLDMRS()
101 bool vpSickLDMRS::setup(
const std::string &ip_address,
int com_port)
103 setIpAddress(ip_address);
105 return (this->setup());
113 bool vpSickLDMRS::setup()
115 struct sockaddr_in serv_addr;
121 socket_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
123 fprintf(stderr,
"Failed to create socket\n");
127 memset(&serv_addr, 0,
sizeof(serv_addr));
128 serv_addr.sin_family = AF_INET;
129 serv_addr.sin_addr.s_addr = inet_addr(ip.c_str());
130 serv_addr.sin_port = htons(port);
133 res = connect(socket_fd, (
struct sockaddr *)&serv_addr,
sizeof(serv_addr));
134 if (errno == EINPROGRESS) {
138 FD_SET(
static_cast<unsigned int>(socket_fd), &myset);
139 res = select(socket_fd + 1,
nullptr, &myset,
nullptr, &tv);
140 if (res < 0 && errno != EINTR) {
141 fprintf(stderr,
"Error connecting to server %d - %s\n", errno, strerror(errno));
145 fprintf(stderr,
"ok");
148 fprintf(stderr,
"Timeout in select() - Cancelling!\n");
162 bool vpSickLDMRS::measure(
vpLaserScan laserscan[4])
164 unsigned int *uintptr;
165 unsigned short *ushortptr;
166 static unsigned char header[24];
167 ushortptr = (
unsigned short *)header;
168 uintptr = (
unsigned int *)header;
170 assert(
sizeof(header) == 24);
173 double time_second = 0;
175 if (isFirstMeasure) {
180 if (recv(socket_fd, header,
sizeof(header), MSG_WAITALL) == -1) {
186 if (ntohl(uintptr[0]) != vpSickLDMRS::MagicWordC2) {
187 printf(
"Error, wrong magic number !!!\n");
192 uint16_t msgtype = ntohs(ushortptr[7]);
193 uint32_t msgLength = ntohl(uintptr[2]);
195 ssize_t len = recv(socket_fd, body, msgLength, MSG_WAITALL);
196 if (len != (ssize_t)msgLength) {
197 printf(
"Error, wrong msg length: %d of %d bytes.\n", (
int)len, msgLength);
201 if (msgtype != vpSickLDMRS::MeasuredData) {
209 unsigned short measurementId;
210 ushortptr = (
unsigned short *)body;
211 measurementId = ushortptr[0];
214 uintptr = (
unsigned int *)(body + 6);
215 unsigned int seconds = uintptr[1];
216 unsigned int fractional = uintptr[0];
217 double startTimestamp = seconds + fractional / 4294967296.;
220 uintptr = (
unsigned int *)(body + 14);
221 seconds = uintptr[1];
222 fractional = uintptr[0];
223 double endTimestamp = seconds + fractional / 4294967296.;
226 if (isFirstMeasure) {
227 time_offset = time_second - startTimestamp;
228 isFirstMeasure =
false;
231 startTimestamp += time_offset;
232 endTimestamp += time_offset;
235 unsigned short numSteps = ushortptr[11];
238 short startAngle = (short)ushortptr[12];
239 short stopAngle = (short)ushortptr[13];
242 unsigned short numPoints = ushortptr[14];
245 for (
int i = 0; i < nlayers; i++) {
246 laserscan[i].
clear();
261 if (numPoints > USHRT_MAX - 2)
264 for (
int i = 0; i < numPoints; i++) {
265 ushortptr = (
unsigned short *)(body + 44 + i * 10);
266 unsigned char layer = ((
unsigned char)body[44 + i * 10]) & 0x0F;
267 unsigned char echo = ((
unsigned char)body[44 + i * 10]) >> 4;
271 hAngle = (2.f * M_PI / numSteps) * (
short)ushortptr[1];
272 rDist = 0.01 * ushortptr[2];
275 scanPoint.
setPolar(rDist, hAngle, vAngle[layer]);
276 laserscan[layer].
addPoint(scanPoint);
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)
static double rad(double deg)
Class that defines a single laser scanner point.
void setPolar(double r_dist, double h_angle, double v_angle)
VISP_EXPORT double measureTimeSecond()