added ble_connect lib and tests

.gitignore

@@ -1 +1,3 @@
 /scripts/qt_pi/*.pyc
+.ccls-cache
+compile_commands.json

CMakeLists.txt

@@ -0,0 +1,3 @@
+cmake_minimum_required(VERSION 3.5)
+ADD_SUBDIRECTORY( qt_pi )
+ADD_SUBDIRECTORY( gazebo_sim )

gazebo_sim/CMakeLists.txt

@@ -5,5 +5,4 @@ set(CMAKE_CXX_FLAGS "-fpermissive -std=c++0x")
 
 find_package(catkin REQUIRED)
 
-catkin_python_setup()
 catkin_package(DEPENDS)

qt_pi/CMakeLists.txt

@@ -11,10 +11,10 @@ catkin_package(DEPENDS)
 find_package(PkgConfig REQUIRED)
 
 find_path(Mcrypt_INCLUDE_DIR mcrypt.h PATHS
-    /usr/local/include
+    /usr/include
 )
 
-set(Mcrypt_LIB_PATHS /usr/local/lib)
+set(Mcrypt_LIB_PATHS /usr/lib)
 find_library(Mcrypt_LIBS NAMES mcrypt rtfilter PATHS ${Mcrypt_LIB_PATHS})
 
 include_directories(include)
@@ -38,5 +38,9 @@ include_directories(${GLIB_INCLUDE_DIRS})
 include_directories(${Mcrypt_INCLUDE_DIRS})
 link_directories(${Mcrypt_LIBS})
 
-add_executable(ble_connect src/Ble_connect.cpp)
+add_library(ble_connect_lib include/ble_connect/ble_connect.h src/ble_connect.c)
-target_link_libraries(ble_connect ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS})
+
+target_link_libraries(ble_connect_lib ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS})
+
+add_executable(ble_connect_test tests/ble_test.cpp)
+target_link_libraries(ble_connect_test ble_connect_lib)

qt_pi/config/70-emotiv.rules

@@ -0,0 +1,7 @@
+SUBSYSTEM=="hidraw", ACTION=="add",  SUBSYSTEMS=="hid", DRIVERS=="generic-usb", KERNEL=="hidraw*", GROUP="username", MODE="0666"
+
+SUBSYSTEM=="hidraw", ACTION=="add",  SUBSYSTEMS=="usb", DRIVERS=="usbhid", GROUP="username", MODE="0666"
+
+SUBSYSTEM=="usb", ACTION=="add", ATTR{idVendor}=="1234", ATTR{idProduct}=="ed02", GROUP="username", MODE="0666"
+
+SUBSYSTEM=="usb", ACTION=="add", SUBSYSTEMS=="usb", ATTRS{idVendor}=="1d6b", GROUP="username", MODE="0666"

qt_pi/doc/Connecting on debian.md

@@ -0,0 +1,80 @@
+## Connecting Emotiv device via USB dongle on Ubuntu 16.04 and Debian 8.3
+
+### Install HID configure for Emotiv USB dongle
+```shell
+sudo cp etc/udev/rules.d/70-emotiv.rules /etc/udev/rules.d/
+sudo service udev restart
+```
+
+Then plugin the USB dongle, you should see the flashing green light and it is ready to be connected to an Emotiv headset.
+
+## Connecting Emotiv device via BTLE on Ubuntu 16.04 and Debian 8.3 with BlueZ library
+### Install and configure latest bluez
+#### Build bluez
+```shell
+sudo apt-get update
+sudo apt-get install libglib2.0-dev libdbus-1-dev libudev-dev libical-dev libreadline-dev
+mkdir -p ~/tmp/bluez && cd ~/tmp/bluez
+wget -c https://www.kernel.org/pub/linux/bluetooth/bluez-5.37.tar.gz
+tar -xvzf bluez-5.37.tar.gz
+cd bluez-5.37/
+./configure --disable-systemd --enable-threads --enable-library
+make
+```
+
+#### Configure
+   * Add `EnableGatt = true` to file `/etc/bluetooth/main.conf`
+   * Backup then edit file `/etc/dbus-1/system.d/bluetooth.conf`, remove `1` from all the lines, such as:
+
+    `<allow send_interface="org.bluez.Agent1"/>` to `<allow send_interface="org.bluez.Agent"/>`
+
+    `<allow send_interface="org.bluez.GattDescriptor1"/>`  to `<allow send_interface="org.bluez.GattDescriptor"/>`
+
+### Run bluez service and test connection
+#### Stop old bluez service
+`sudo /etc/init.d/bluetooth stop`
+
+#### Start new bluez
+`sudo ./src/bluetoothd -n -E --plugin=audio`
+
+#### Test connection
+Open other terminal and run `bluetoothctl`. Use following commands to test connection:
+* List available Bluetooth adapters
+
+  `[bluetooth]#list`
+
+* Power on Bluetooth adapter
+
+  `[bluetooth]#power on`
+
+* Set Bluetooth adapter discoverable
+
+  `[bluetooth]#discoverable on`
+
+* Set Bluetooth adapter pairable
+
+  `[bluetooth]#pairable on`
+
+* Tell Bluetooth adapter to scan Bluetooth devices
+
+  `[bluetooth]#scan on`
+
+#### List and connect to device
+* List all available bluetooth devices
+
+  `[bluetooth]#devices`
+
+* Get device info
+
+  `[bluetooth]#info [MAC_Address]`
+
+  > Each Emotiv device has a specific service UUID `81072f40-9f3d-11e3-a9dc-0002a5d5c51b`
+
+* Connect to an Emotiv device
+
+  `[bluetooth]#connect [MAC Address]`
+
+Now you can run the SDK or app on Ubuntu - enjoy!
+
+#### Using graphical view
+To use the graphical view instead of command line, run `d-feet &` then select `/org.bluez/hci0/[MAC_Address]`

qt_pi/include/ble_connect/ble_connect.h

@@ -0,0 +1,33 @@
+#ifndef BLE_CONNECT
+#define BLE_CONNECT
+#include "gattlib.h"
+#include <glib.h>
+#include <mcrypt.h>
+#include <pthread.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+enum Channels { AF3 = 0, T7 = 1, Pz = 2, T8 = 3, AF4 = 4 };
+
+struct insight_data {
+  double uV[5];
+  struct timespec ts;
+} insight_data;
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern int insight_init(const char *a, const char *b);
+extern int insight_destroy();
+extern int insight_get_status();
+extern int insight_start_notif();
+extern int insight_stop_notif();
+extern void insight_write_to_buffer(int len, struct insight_data *buf);
+extern int insight_buffer_status();
+extern int insight_stop_write_to_buffer();
+extern int insight_version();
+#ifdef __cplusplus
+}
+#endif
+#endif

qt_pi/src/Ble_connect.cpp

@@ -1,136 +0,0 @@
-#include <glib.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <iostream>
-#include <mcrypt.h>
-#include <pthread.h>
-
-#include <string.h>
-
-#include "gattlib.h"
-
-static uuid_t g_uuid;
-const uint8_t BT_key[4] = {0x5a, 0x68, 0x5b, 0xb6};
-const uint8_t key[16] = {BT_key[0], 00, BT_key[1], 21, BT_key[2], 00, BT_key[3], 12, BT_key[2], 00, BT_key[1], 68, BT_key[0], 00, BT_key[1], 88 };
-static uint8_t data[16];
-int data_length = 16;
-
-MCRYPT mcrpt;
-double uV[5];
-uint8_t raw_data[72];
-enum Channels{
-    AF3 = 0,
-    T7  = 1,
-    Pz  = 2,
-    T8  = 3,
-    AF4 = 4
-};
-
-int k = 0;
-
-double raw(uint8_t h, uint8_t l){
-    return (((double)h - 128) * 32.82051289 + (double)l * -0.128205128205129 + 4201.02564096001);
-}
-
-void notification_handler(const uuid_t* uuid, const uint8_t* val, size_t values_length, void* user_data) {
-	int i, j, k;
-    //for(i = 0; i < values_length; i++)
-    //    std::cout<<(int)val[i]<<"\t";
-    //std::cout<<std::endl;
-    mdecrypt_generic (mcrpt, val, data_length);
-    for(i = 0; i < 9; i++)
-    {
-        for(k = 7; k >= 0; k--)
-            raw_data[i * 8 + (7 - k)] = (val[i] >> k) & 1;
-    }
-
-    for(i = 0; i < 5; i++)
-    {
-        uint8_t h = 0, l = 0;
-
-        j=-1;
-        while((++j) < 8)
-            h = (h << 1) | ((uint8_t)(raw_data[(14 * i ) + j] ));
-        j--;
-        while((++j) < 14)
-            l = (l << 1) | ((uint8_t)(raw_data[(14 * i ) + j]));
-
-        uV[i] = raw(h,l);
-    }
-
-    //for (i = 0; i < data_length; i++) {
-		//if(val[9] == 194 || val[9]==0)continue;
-        //std::cout<<std::dec<<(int)val[9]<<" ";
-		//std::cout<<std::dec<<(int)val[i]<<"\t";
-	//}
-
-    std::cout.precision(10);
-	std::cout<<k<<"\t";
-	for (i = 0; i < 5; i++) {
-		std::cout<<uV[i]<<"\t";
-	}
-
-    /*int n=((val[1] & 0b00000011) << 5 ) + (val[2] >> 3);
-    for (c = 7; c >= 0; c--)
-  {
-    k = n >> c;
-    if (k & 1)
-      printf("1");
-    else
-      printf("0");
-  }*/
-    printf("\n");
-	//if(val[0]==0)printf("\n");
-}
-
-static void usage(char *argv[]) {
-	printf("%s <device_address> <UUID>\n", argv[0]);
-}
-
-GMainLoop *loop;
-
-void run(){
-    loop = g_main_loop_new(NULL, 0);
-    g_main_loop_run(loop);
-    pthread_exit(NULL);
-}
-
-int main(int argc, char *argv[]) {
-
-    mcrpt = mcrypt_module_open(MCRYPT_RIJNDAEL_128, NULL, MCRYPT_ECB, NULL);
-    mcrypt_generic_init(mcrpt, key, data_length, NULL);
-	int ret;
-    gatt_connection_t* connection;
-
-
-	if (argc != 3) {
-		usage(argv);
-		return 1;
-	}
-
-	if (gattlib_string_to_uuid(argv[2], strlen(argv[2]) + 1, &g_uuid) < 0) {
-		usage(argv);
-		return 1;
-	}
-	connection = gattlib_connect(NULL, argv[1], BDADDR_LE_PUBLIC, BT_SEC_LOW, 0, 0);
-	if (connection == NULL) {
-		fprintf(stderr, "Fail to connect to the bluetooth device.\n");
-		return 1;
-	}
-	gattlib_register_notification(connection, notification_handler, NULL);
-	ret = gattlib_notification_start(connection, &g_uuid);
-	if (ret) {
-		fprintf(stderr, "Fail to start notification\n.");
-		return 1;
-	}
-    pthread_t thread;
-    pthread_create(&thread, NULL,run,NULL);
-	getchar();
-    g_main_loop_quit(loop);
-	g_main_loop_unref(loop);
-	gattlib_notification_stop(connection, &g_uuid);
-	gattlib_disconnect(connection);
-    mcrypt_generic_deinit (mcrpt);
-	mcrypt_module_close(mcrpt);
-	return 0;
-}

qt_pi/src/ble_connect.c

@@ -0,0 +1,160 @@
+#include "ble_connect/ble_connect.h"
+
+#define B0 0x5a
+#define B1 0x68
+#define B2 0x5b
+#define B3 0xb6
+
+MCRYPT mcrpt;
+GMainLoop *loop;
+gatt_connection_t *connection;
+const int data_length = 16;
+uint8_t key[16] = {B0, 00, B1, 21, B2, 00, B3, 12,
+                   B2, 00, B1, 68, B0, 00, B1, 88};
+static uint8_t data[16];
+static uuid_t g_uuid;
+
+static uint8_t raw_data[72];
+
+struct insight_data *data_buffer;
+int data_buffer_length;
+int data_index;
+
+int status = 0;
+int write_to_buffer = 0;
+
+pthread_t thread;
+
+double raw(uint8_t h, uint8_t l) {
+  return (((double)h - 128) * 32.82051289 + (double)l * -0.128205128205129 +
+          4201.02564096001);
+}
+
+void notification_handler(const uuid_t *uuid, const uint8_t *val,
+                          size_t values_length, void *user_data) {
+
+  if (write_to_buffer == 0 || write_to_buffer == 2)
+    return;
+
+  timespec_get(&data_buffer[data_index].ts, TIME_UTC);
+
+  mdecrypt_generic(mcrpt, (void *)val, data_length);
+
+  for (int i = 0; i < 9; i++) {
+    for (int k = 7; k >= 0; k--)
+      raw_data[i * 8 + (7 - k)] = (val[i] >> k) & 1;
+  }
+
+  for (int i = 0; i < 5; i++) {
+    uint8_t h = 0, l = 0;
+
+    int j = -1;
+    while ((++j) < 8)
+      h = (h << 1) | ((uint8_t)(raw_data[(14 * i) + j]));
+    j--;
+    while ((++j) < 14)
+      l = (l << 1) | ((uint8_t)(raw_data[(14 * i) + j]));
+
+    data_buffer[data_index].uV[i] = raw(h, l);
+  }
+
+  data_index++;
+
+  if (data_index == data_buffer_length)
+    write_to_buffer = 2;
+}
+
+void insight_write_to_buffer(int len, struct insight_data *buf) {
+  data_buffer = buf;
+  data_buffer_length = len;
+  data_index = 0;
+  write_to_buffer = 1;
+}
+
+int insight_stop_write_to_buffer() {
+  write_to_buffer = 0;
+  return data_index;
+}
+
+int insight_buffer_status() { return write_to_buffer; }
+
+void run() {
+  g_main_loop_run(loop);
+  pthread_exit(NULL);
+}
+
+int insight_init(const char *a, const char *b) {
+
+  if (status != 0)
+    return 1;
+
+  mcrpt = mcrypt_module_open(MCRYPT_RIJNDAEL_128, NULL, MCRYPT_ECB, NULL);
+  mcrypt_generic_init(mcrpt, (void *)key, data_length, NULL);
+
+  if (gattlib_string_to_uuid(b, strlen(b) + 1, &g_uuid) < 0) {
+    return 2;
+  }
+
+  connection = gattlib_connect(NULL, a, BDADDR_LE_PUBLIC, BT_SEC_LOW, 0, 0);
+
+  if (connection == NULL) {
+    return 3;
+  }
+
+  gattlib_register_notification(connection, notification_handler, NULL);
+  loop = g_main_loop_new(NULL, 0);
+
+  status = 1;
+
+  return 0;
+}
+
+int insight_start_notif() {
+
+  if (status != 1)
+    return 2;
+
+  write_to_buffer = 0;
+  int ret = gattlib_notification_start(connection, &g_uuid);
+
+  if (ret)
+    return ret;
+
+  pthread_create(&thread, NULL, (void *(*)(void *))run, NULL);
+
+  while (!g_main_loop_is_running(loop))
+    sleep(0);
+
+  status = 2;
+  return 0;
+}
+
+int insight_stop_notif() {
+  if (status != 2)
+    return 1;
+
+  write_to_buffer = 0;
+
+  g_main_loop_quit(loop);
+  gattlib_notification_stop(connection, &g_uuid);
+  pthread_join(thread, NULL);
+
+  status = 1;
+  return 0;
+}
+
+int insight_destroy() {
+  if (status != 1)
+    return status;
+
+  g_main_loop_unref(loop);
+  gattlib_disconnect(connection);
+  mcrypt_generic_deinit(mcrpt);
+  mcrypt_module_close(mcrpt);
+  status = 0;
+  return 0;
+}
+
+int insight_get_status() { return status; }
+
+int insight_version() { return 0; }

qt_pi/tests/ble_test.cpp

@@ -0,0 +1,31 @@
+#include "ble_connect/ble_connect.h"
+#include <unistd.h>
+int main() {
+  const char *a = "EF:BD:39:4E:08:49";
+  const char *b = "81072f41-9f3d-11e3-a9dc-0002a5d5c51b";
+
+  insight_init(a, b);
+  insight_start_notif();
+
+  struct insight_data *buf =
+      (struct insight_data *)malloc(sizeof(struct insight_data) * 10);
+
+  int len = 10;
+
+  insight_write_to_buffer(len, buf);
+
+  while (insight_buffer_status() == 1)
+    sleep(0);
+
+  // sleep(5);
+
+  printf("Stopping notif %d \n", insight_stop_notif());
+
+  for (int i = 0; i < len; i++) {
+    printf("%f\n", buf[i].uV[0]);
+  }
+
+  printf("Destroy called %d\n", insight_destroy());
+  printf("%d\n", insight_version());
+  return 0;
+}