--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <libusb-1.0/libusb.h>
+#include <robottype.h>
+#include <roboorte_robottype.h>
+
+/**
+ * Function prototypes.
+ */
+ssize_t getSOPASDeviceList(libusb_context *ctx, uint16_t vendorID, uint16_t productID, libusb_device ***list);
+void freeSOPASDeviceList(libusb_device **list);
+void printUSBDeviceDetails(struct libusb_device_descriptor desc);
+void printUSBInterfaceDetails(libusb_device* device);
+
+void printSOPASDeviceInformation(ssize_t numberOfDevices, libusb_device** devices);
+int sendSOPASTelegram(libusb_device_handle* device_handle, const char* request, unsigned int timeout);
+int receiveSOPASTelegram(libusb_device_handle* device_handle, unsigned char* receiveBuffer, int receiveBufferSize, unsigned int timeout);
+int read_scan(libusb_device_handle* device_handle, unsigned short * ranges, unsigned short * intensities, int size);
+
+int main(void) {
+
+ printf("SICK TIM3xx daemon\n\n");
+
+ /*
+ * Create and initialize a new LIBUSB session.
+ */
+ libusb_context *ctx = NULL;
+ int result = libusb_init(&ctx);
+ if (result != 0) {
+ fprintf(stderr,"LIBUSB - Initialization failed with the following error code: %i.\n", result);
+ }
+
+ /*
+ * Set the verbosity level to 3 as suggested in the documentation.
+ */
+ libusb_set_debug(ctx, 3);
+
+ /*
+ * Get a list of all SICK TIM3xx devices connected to the USB bus.
+ *
+ * As a shortcut, you can also use the LIBUSB function:
+ * libusb_open_device_with_vid_pid(ctx, 0x19A2, 0x5001).
+ */
+ libusb_device **devices = NULL;
+ int vendorID = 0x19A2; // SICK AG
+ int deviceID = 0x5001; // TIM3XX
+ ssize_t numberOfDevices = getSOPASDeviceList(ctx, vendorID, deviceID, &devices);
+
+ /*
+ * If available, open the first SICK TIM3xx device.
+ */
+ if (numberOfDevices > 0) {
+
+ /*
+ * Print out the SOPAS device information to the console.
+ */
+ printSOPASDeviceInformation(numberOfDevices, devices);
+
+ /*
+ * Open the device handle and detach all kernel drivers.
+ */
+ libusb_device_handle *device_handle;
+ libusb_open(devices[0], &device_handle);
+ if (device_handle == NULL) {
+ fprintf(stderr, "\nLIBUSB - Cannot open device\n");
+ } else {
+ printf("\nLIBUSB - Device opened\n");
+ }
+
+ if (libusb_kernel_driver_active(device_handle, 0) == 1) {
+ printf("LIBUSB - Kernel driver active\n");
+ if (libusb_detach_kernel_driver(device_handle, 0) == 0) {
+ printf("LIBUSB - Kernel driver detached!\n");
+ }
+ }
+
+ /*
+ * Claim the interface 0
+ */
+ result = libusb_claim_interface(device_handle, 0);
+ if (result < 0) {
+ fprintf(stderr, "LIBUSB - Cannot claim interface\n");
+ } else {
+ printf("LIBUSB - Claimed interface\n");
+ }
+
+ /*
+ * Read the SOPAS variable 'LMDscandata' by name.
+ */
+// const char requestScanData[] = { "\x02sRN LMDscandata\x03\0" };
+// unsigned char receiveBuffer[65536];
+//
+// result = sendSOPASTelegram(device_handle, requestScanData, 500);
+// if (result != 0) {
+// fprintf(stderr, "SOPAS - Error sending command.\n");
+// }
+//
+// result = receiveSOPASTelegram(device_handle, receiveBuffer,65535, 500);
+// if (result != 0) {
+// fprintf(stderr, "SOPAS - Error receiving command.\n");
+// }
+//
+// printf("LIBUSB - Read data... %s\n\n", receiveBuffer);
+
+ unsigned short ranges[271];
+ unsigned short intensities[271];
+ read_scan(device_handle, ranges, intensities, 271);
+
+ /*
+ * Release the interface
+ */
+ result = libusb_release_interface(device_handle, 0);
+ if (result != 0) {
+ fprintf(stderr, "LIBUSB - Cannot Release Interface\n");
+ }
+ printf("LIBUSB - Released Interface\n");
+
+ /*
+ * Close the device handle.
+ */
+ libusb_close(device_handle);
+ }
+ else
+ {
+ fprintf(stderr, "Error, no SICK Tim3xx device found\n\n");
+ }
+
+ /*
+ * Free the list of the USB devices.
+ */
+ freeSOPASDeviceList(devices);
+
+ /*
+ * Close the LIBUSB session.
+ */
+ libusb_exit(ctx);
+
+ printf("SICK TIM3xx daemon finished\n\n");
+
+ return EXIT_SUCCESS;
+}
+
+int read_scan(libusb_device_handle* device_handle, unsigned short * ranges, unsigned short * intensities, int size)
+{
+ int result = 0;
+ unsigned char receiveBuffer[65536];
+ unsigned char receiveBufferCopy[65536]; // only for debugging
+ int actual_length = 0;
+ static const size_t NUM_FIELDS = 580;
+ char* fields[NUM_FIELDS];
+ size_t count;
+ static unsigned int iteration_count = 0;
+
+ result = libusb_bulk_transfer(device_handle, (1 | LIBUSB_ENDPOINT_IN), receiveBuffer, 65535, &actual_length, 0);
+ if (result != 0)
+ {
+ if (result == LIBUSB_ERROR_TIMEOUT)
+ {
+ fprintf(stderr, "LIBUSB - Read Error: LIBUSB_ERROR_TIMEOUT.");
+ return EXIT_SUCCESS; // return success to continue looping
+ }
+ else
+ {
+ fprintf(stderr, "LIBUSB - Read Error: %i.", result);
+ return EXIT_FAILURE;
+ }
+ }
+
+ receiveBuffer[actual_length] = 0;
+ //printf("LIBUSB - Read data... %s", receiveBuffer);
+
+ // ----- tokenize
+ //strncpy((char*)receiveBufferCopy, (char*)receiveBuffer, 65535); // receiveBuffer will be changed by strtok
+ //receiveBufferCopy[65535] = 0;
+
+ count = 0;
+ fields[count] = strtok((char *)receiveBuffer, " ");
+ // ROS_DEBUG("%d: %s ", count, fields[count]);
+
+ while (fields[count] != NULL)
+ {
+ count++;
+ if (count > NUM_FIELDS)
+ break;
+
+ fields[count] = strtok(NULL, " ");
+ // ROS_DEBUG("%d: %s ", count, fields[count]);
+ }
+
+ if (count < NUM_FIELDS)
+ {
+ fprintf(stderr,
+ "received less fields than expected fields (actual: %zu, expected: %zu), ignoring scan", count, NUM_FIELDS);
+ // ROS_DEBUG("received message was: %s", receiveBufferCopy);
+ return EXIT_SUCCESS; // return success to continue looping
+ }
+ else if (count > NUM_FIELDS)
+ {
+ fprintf(stderr, "received more fields than expected (expected: %zu), ignoring scan", NUM_FIELDS);
+ // ROS_DEBUG("received message was: %s", receiveBufferCopy);
+ return EXIT_SUCCESS; // return success to continue looping
+ }
+
+ // ----- read fields into msg
+ //sensor_msgs::LaserScan msg;
+
+ // <STX> (\x02)
+ // 0: Type of command (SN)
+ // 1: Command (LMDscandata)
+ // 2: Firmware version number (1)
+ // 3: Device number (1)
+ // 4: Serial number (B96518)
+ // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
+ // 7: Telegram counter (99)
+ // 8: Scan counter (9A)
+ // 9: Time since startup (13C8E59)
+ // 10: Time of transmission (13C9CBE)
+ // 11 + 12: Input status (0 0)
+ // 13 + 14: Output status (8 0)
+ // 15: Reserved Byte A (0)
+
+ // 16: Scanning Frequency (5DC)
+ unsigned short scanning_freq = -1;
+ sscanf(fields[16], "%hx", &scanning_freq);
+ //msg.scan_time = 1.0 / (scanning_freq / 100.0);
+ // ROS_DEBUG("hex: %s, scanning_freq: %d, scan_time: %f", fields[16], scanning_freq, msg.scan_time);
+
+ // 17: Measurement Frequency (36)
+ unsigned short measurement_freq = -1;
+ sscanf(fields[17], "%hx", &measurement_freq);
+ //msg.time_increment = 1.0 / (measurement_freq * 100.0);
+ // ROS_DEBUG("measurement_freq: %d, time_increment: %f", measurement_freq, msg.time_increment);
+
+ // 18: Number of encoders (0)
+ // 19: Number of 16 bit channels (1)
+ // 20: Measured data contents (DIST1)
+
+ // 21: Scaling factor (3F800000)
+ // ignored for now (is always 1.0):
+// unsigned int scaling_factor_int = -1;
+// sscanf(fields[21], "%x", &scaling_factor_int);
+//
+// float scaling_factor = reinterpret_cast<float&>(scaling_factor_int);
+// // ROS_DEBUG("hex: %s, scaling_factor_int: %d, scaling_factor: %f", fields[21], scaling_factor_int, scaling_factor);
+
+ // 22: Scaling offset (00000000) -- always 0
+ // 23: Starting angle (FFF92230)
+ int starting_angle = -1;
+ sscanf(fields[23], "%x", &starting_angle);
+ //msg.angle_min = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
+ // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
+
+ // 24: Angular step width (2710)
+ unsigned short angular_step_width = -1;
+ sscanf(fields[24], "%hx", &angular_step_width);
+ //msg.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
+ //msg.angle_max = msg.angle_min + 270.0 * msg.angle_increment;
+
+ // adjust angle_min to min_ang config param
+
+ int index_min = 0;
+// while (msg.angle_min + msg.angle_increment < config_.min_ang)
+// {
+// msg.angle_min += msg.angle_increment;
+// index_min++;
+// }
+
+ // adjust angle_max to max_ang config param
+ int index_max = 270;
+ if (size <= index_max)
+ {
+ fprintf(stderr, "Size of fields ranges and intensities is small!");
+ return 1;
+ }
+// while (msg.angle_max - msg.angle_increment > config_.max_ang)
+// {
+// msg.angle_max -= msg.angle_increment;
+// index_max--;
+// }
+
+ //ROS_DEBUG("index_min: %d, index_max: %d", index_min, index_max);
+ // ROS_DEBUG("angular_step_width: %d, angle_increment: %f, angle_max: %f", angular_step_width, msg.angle_increment, msg.angle_max);
+
+ // 25: Number of data (10F)
+
+ // 26..296: Data_1 .. Data_n
+ //msg.ranges.resize(index_max - index_min + 1);
+ int j;
+ for (j = index_min; j <= index_max; ++j)
+ {
+ unsigned short range;
+ sscanf(fields[j + 26], "%hx", &range);
+ ranges[j - index_min] = range; // /1000.0;
+ }
+
+ // 297: Number of 8 bit channels (1)
+ // 298: Measured data contents (RSSI1)
+ // 299: Scaling factor (3F800000)
+ // 300: Scaling offset (00000000)
+ // 301: Starting angle (FFF92230)
+ // 302: Angular step width (2710)
+ // 303: Number of data (10F)
+ // 304..574: Data_1 .. Data_n
+ if (intensities != NULL)
+ {
+ //msg.intensities.resize(index_max - index_min + 1);
+ int j;
+ for (j = index_min; j <= index_max; ++j)
+ {
+ unsigned short intensity;
+ sscanf(fields[j + 304], "%hx", &intensity);
+ intensities[j - index_min] = intensity;
+ }
+ }
+
+ // 575: Position (0)
+ // 576: Name (0)
+ // 577: Comment (0)
+ // 578: Time information (0)
+ // 579: Event information (0)
+ // <ETX> (\x03)
+
+ //msg.range_min = 0.05;
+ //msg.range_max = 4.0;
+
+ // ----- adjust start time
+ // - last scan point = now ==> first scan point = now - 271 * time increment
+ //msg.header.stamp = start_time - ros::Duration().fromSec(271 * msg.time_increment);
+
+ // - shift forward to time of first published scan point
+ //msg.header.stamp += ros::Duration().fromSec((double)index_min * msg.time_increment);
+
+ // - add time offset (to account for USB latency etc.)
+ //msg.header.stamp += ros::Duration().fromSec(config_.time_offset);
+
+ //pub_.publish(msg);
+ return EXIT_SUCCESS;
+}
+
+/**
+ * Returns a list of USB devices currently attached to the system and matching the given vendorID and productID.
+ */
+ssize_t getSOPASDeviceList(libusb_context *ctx, uint16_t vendorID, uint16_t productID, libusb_device ***list) {
+
+ libusb_device **resultDevices = NULL;
+ ssize_t numberOfResultDevices = 0;
+
+ libusb_device **devices;
+ ssize_t numberOfDevices;
+
+ /*
+ * Get a list of all USB devices connected.
+ */
+ numberOfDevices = libusb_get_device_list(ctx, &devices);
+
+ /*
+ * Iterate through the list of the connected USB devices and search for devices with the given vendorID and prodcutID.
+ */
+ ssize_t i;
+ for (i = 0; i < numberOfDevices; i++) {
+
+ struct libusb_device_descriptor desc;
+ int result = libusb_get_device_descriptor(devices[i], &desc);
+ if (result < 0) {
+ fprintf(stderr, "LIBUSB - Failed to get device descriptor");
+ }
+
+ if (desc.idVendor == vendorID && desc.idProduct == 0x5001) {
+
+ /*
+ * Add the matching device to the function result list and increase the device reference count.
+ */
+ resultDevices = realloc(resultDevices, sizeof(libusb_device *) + (numberOfResultDevices + 1));
+ if (resultDevices == NULL) {
+ fprintf(stderr, "LIBUSB - Failed to allocate memory for the device result list.");
+ } else {
+ resultDevices[numberOfResultDevices] = devices[i];
+ resultDevices[numberOfResultDevices + 1] = NULL;
+ libusb_ref_device(devices[i]);
+ numberOfResultDevices++;
+ }
+ }
+ }
+
+ /*
+ * Free the list of the connected USB devices and decrease the device reference count.
+ */
+ libusb_free_device_list(devices, 1);
+
+ /*
+ * Prepare the return values of the function.
+ */
+ *list = resultDevices;
+ return numberOfResultDevices;
+}
+
+/*
+ * Free the list of devices obtained from the function 'getSOPASDeviceList'.
+ */
+void freeSOPASDeviceList(libusb_device **list) {
+
+ if (!list)
+ return;
+
+ int i = 0;
+ struct libusb_device *dev;
+ while ((dev = list[i++]) != NULL)
+ libusb_unref_device(dev);
+
+ free(list);
+}
+
+/*
+ * Print the device details such as USB device class, vendor id and product id to the console.
+ */
+void printUSBDeviceDetails(struct libusb_device_descriptor desc) {
+
+ printf("Device Class: 0x%x\n", desc.bDeviceClass);
+ printf("VendorID: 0x%x\n", desc.idVendor);
+ printf("ProductID: 0x%x\n", desc.idProduct);
+}
+
+
+/**
+ * Print the USB device information of the connected TIM3xx devices to the console.
+ */
+void printSOPASDeviceInformation(ssize_t numberOfDevices, libusb_device** devices) {
+ ssize_t i;
+ for (i = 0; i < numberOfDevices; i++) {
+ struct libusb_device_descriptor desc;
+ int result = libusb_get_device_descriptor(devices[i], &desc);
+ if (result < 0) {
+ fprintf(stderr, "LIBUSB - Failed to get device descriptor");
+ }
+ if (result == 0) {
+ printf("SICK AG - TIM3XX - [%i]\n", ((int)i + 1));
+ printf("----------------------------------------\n");
+ printUSBDeviceDetails(desc);
+ printf("----------------------------------------\n");
+ }
+ }
+
+ if (numberOfDevices == 0) {
+ printf("LIBUSB - No SICK TIM3xx device connected.\n");
+ }
+}
+
+/**
+ * Send a SOPAS command to the device.
+ * Example of the request: const char requestScanData[] = { "\x02sRN LMDscandata\x03\0" };
+ * \x02 - STX (Start of TeXt, start of the telegram)
+ * sRN - Read by Name
+ * LMDscandata - request for measured data
+ * \x03 - ETX (End of TeXt, end of the telegram)
+ * \0 - end of string
+ * For more details, see document "Telegrams for Configuring and Operating the LMS1xx, LMS5xx,
+ * TiM3xx, JEF300, JEF500".
+ */
+int sendSOPASTelegram(libusb_device_handle* device_handle, const char* request, unsigned int timeout) {
+
+ int result = 0;
+
+ /*
+ * Write a SOPAS variable read request to the device.
+ */
+ printf("\nLIBUSB - Write data... %s\n", request);
+
+ int actual = 0;
+ int requestLength = strlen(request);
+ result = libusb_bulk_transfer(device_handle, (2 | LIBUSB_ENDPOINT_OUT), (unsigned char*) request, requestLength, &actual, 0);
+ if (result != 0 || actual != requestLength) {
+ fprintf(stderr, "LIBUSB - Write Error: %i.\n", result);
+ }
+
+ return result;
+}
+
+/**
+ * Receive a SOPAS telegram from the device.
+ * For more details, see document "Telegrams for Configuring and Operating the LMS1xx, LMS5xx,
+ * TiM3xx, JEF300, JEF500".
+ */
+int receiveSOPASTelegram(libusb_device_handle* device_handle, unsigned char* receiveBuffer, int receiveBufferSize, unsigned int timeout) {
+
+ int result = 0;
+ int actual = 0;
+
+ /*
+ * Read the SOPAS device response with the given timeout.
+ */
+ result = libusb_bulk_transfer(device_handle, (1 | LIBUSB_ENDPOINT_IN), receiveBuffer, receiveBufferSize, &actual, timeout);
+ if (result != 0) {
+ fprintf(stderr, "LIBUSB - Read Error: %i.\n", result);
+ }
+
+ // Place zero on the end of the string
+ receiveBuffer[actual] = 0;
+
+ return result;
+}
\ No newline at end of file
projects / eurobot / public.git / commitdiff