#include <libusb-1.0/libusb.h>
#include <robottype.h>
#include <roboorte_robottype.h>
+#include <signal.h>
+
+static unsigned short *dists = NULL; // distance scan
+//static unsigned short *refls = NULL; // reflex instensity scan
+static int scan_max = 271; // angle range 0 - 270, step is 1 degree
+int interrupt = 0;
-/**
- * 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 read_scan(libusb_device_handle* device_handle, unsigned short* ranges, unsigned short* intensities, int size);
+static void sig_handler(int sig);
int main(void) {
- printf("SICK TIM3xx daemon\n\n");
+ printf("sickd: This daemon reads data from SICK TIM3xx and publics it on ORTE.\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);
+ fprintf(stderr,"sickd: 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);
+ //libusb_set_debug(ctx, 3);
/*
* Get a list of all SICK TIM3xx devices connected to the USB bus.
int vendorID = 0x19A2; // SICK AG
int deviceID = 0x5001; // TIM3XX
ssize_t numberOfDevices = getSOPASDeviceList(ctx, vendorID, deviceID, &devices);
+
+ // ORTE init
+ struct robottype_orte_data orte;
+
+ orte.strength = 1;
+
+ if(robottype_roboorte_init(&orte)) {
+ fprintf(stderr,"sickd: Roboorte initialization failed! Exiting...\n");
+ exit(-1);
+ }
+ else
+ printf("sickd: Roboorte OK\n");
+ // Creating ORTE publishers
+ robottype_publisher_sick_scan_create(&orte, NULL, NULL);
+ printf("sickd: Publisher OK\n");
+
+ // Signal handler registration
+ signal(SIGINT, sig_handler);
+ signal(SIGTERM, sig_handler);
+
/*
* If available, open the first SICK TIM3xx device.
*/
libusb_device_handle *device_handle;
libusb_open(devices[0], &device_handle);
if (device_handle == NULL) {
- fprintf(stderr, "\nLIBUSB - Cannot open device\n");
+ fprintf(stderr, "sickd: LIBUSB - Cannot open device\n");
} else {
- printf("\nLIBUSB - Device opened\n");
+ printf("sickd: LIBUSB - Device opened\n");
}
if (libusb_kernel_driver_active(device_handle, 0) == 1) {
- printf("LIBUSB - Kernel driver active\n");
+ printf("sickd: LIBUSB - Kernel driver active\n");
if (libusb_detach_kernel_driver(device_handle, 0) == 0) {
- printf("LIBUSB - Kernel driver detached!\n");
+ printf("sickd: LIBUSB - Kernel driver detached!\n");
}
}
*/
result = libusb_claim_interface(device_handle, 0);
if (result < 0) {
- fprintf(stderr, "LIBUSB - Cannot claim interface\n");
+ fprintf(stderr, "sickd: LIBUSB - Cannot claim interface\n");
} else {
- printf("LIBUSB - Claimed interface\n");
+ printf("sickd: LIBUSB - Claimed interface\n");
}
/*
//
// printf("LIBUSB - Read data... %s\n\n", receiveBuffer);
- unsigned short ranges[271];
- unsigned short intensities[271];
- read_scan(device_handle, ranges, intensities, 271);
-
- printf("Ranges a intensities:\n");
- int i;
- for (i = 0; i < 271; i++)
- {
- printf("index: %d, range: %d, intensity: %d\n", i, ranges[i], intensities[i]);
- };
+ /*
+ * Start streaming 'LMDscandata'.
+ */
+ const char scan_start_cmd[] = {"\x02sEN LMDscandata 1\x03\0"};
+ result = sendSOPASTelegram(device_handle, scan_start_cmd, 500);
+ if (result != 0)
+ {
+ fprintf(stderr, "sickd: SOPAS - Error starting to stream 'LMDscandata'.");
+ return EXIT_FAILURE;
+ }
+ // Distance scan variable init
+ dists = (unsigned short*)malloc(sizeof(unsigned short) * scan_max);
+ if (dists == NULL) {
+ fprintf(stderr, "sickd: Malloc failed!\n");
+ return -1;
+ }
+
+ // Reflex instensity scan variable init
+// refls = (unsigned short*)malloc(sizeof(unsigned short) * scan_max);
+// if (refls == NULL) {
+// fprintf(stderr, "sickd: Malloc failed!\n");
+// return -1;
+// }
+
+ while (!interrupt) {
+ //read_scan(device_handle, dists, refls, scan_max);
+
+ // Read distances only
+ read_scan(device_handle, dists, NULL, scan_max);
+
+ // Print measured data
+ printf("Ranges:\n");
+
+ int j;
+ for (j = 0; j < scan_max; j++) {
+ //printf("index: %d, distance: %li, reflex intensity: %li\n", j, dists[j], refls[j]);
+ printf("index: %d, distance: %d\n", j, dists[j]);
+ orte.sick_scan.data[j] = dists[j];
+ /*if (refls) {
+ orte.sick_scan.data[j + scan_max] = refls[j];
+ }*/
+ };
+
+
+ // Publish on ORTE
+ ORTEPublicationSend(orte.publication_sick_scan);
+ }
+
+ /*
+ * End streaming 'LMDscandata'.
+ */
+ const char scan_stop_cmd[] = {"\x02sEN LMDscandata 0\x03\0"};
+ result = sendSOPASTelegram(device_handle, scan_stop_cmd, 500);
+ if (result != 0)
+ {
+ fprintf(stderr, "sickd: SOPAS - Error ending to stream 'LMDscandata'.");
+ return EXIT_FAILURE;
+ }
+
/*
* Release the interface
*/
result = libusb_release_interface(device_handle, 0);
if (result != 0) {
- fprintf(stderr, "LIBUSB - Cannot Release Interface\n");
+ fprintf(stderr, "sickd: LIBUSB - Cannot Release Interface\n");
}
- printf("LIBUSB - Released Interface\n");
+ printf("sickd: LIBUSB - Released Interface\n");
/*
* Close the device handle.
}
else
{
- fprintf(stderr, "Error, no SICK Tim3xx device found\n\n");
+ fprintf(stderr, "sickd: Error, no SICK Tim3xx device found\n\n");
}
+
+ // ORTE deinit
+ robottype_roboorte_destroy(&orte);
/*
* Free the list of the USB devices.
*/
libusb_exit(ctx);
- printf("SICK TIM3xx daemon finished\n\n");
+ printf("sickd: SICK TIM3xx daemon finished\n\n");
return EXIT_SUCCESS;
}
{
int result = 0;
unsigned char receiveBuffer[65536];
- unsigned char receiveBufferCopy[65536]; // only for debugging
+ //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;
+ //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.");
+ fprintf(stderr, "sickd: LIBUSB - Read Error: LIBUSB_ERROR_TIMEOUT.");
return EXIT_SUCCESS; // return success to continue looping
}
else
{
- fprintf(stderr, "LIBUSB - Read Error: %i.", result);
+ fprintf(stderr, "sickd: LIBUSB - Read Error: %i.", result);
return EXIT_FAILURE;
}
}
if (count < NUM_FIELDS)
{
fprintf(stderr,
- "received less fields than expected fields (actual: %zu, expected: %zu), ignoring scan", count, NUM_FIELDS);
+ "sickd: 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);
+ fprintf(stderr, "sickd: 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
}
{
unsigned short range;
sscanf(fields[j + 26], "%hx", &range);
- ranges[j - index_min] = range; // /1000.0;
+ ranges[j - index_min] = range; //distance in mm
}
// 297: Number of 8 bit channels (1)
receiveBuffer[actual] = 0;
return result;
+}
+
+static void sig_handler(int sig)
+{
+ interrupt = 1;
}
\ No newline at end of file
projects / eurobot / public.git / commitdiff