mirror of
https://gitlab.com/yikestone/qt_pi.git
synced 2025-08-05 22:54:11 +05:30
Merge remote-tracking branch 'origin/master' into temp_branch
This commit is contained in:
Rishabh Kundu
commit
868eee170d
4
.gitignore
vendored
4
.gitignore
vendored
@ -1 +1,3 @@
|
|||||||
/scripts/qt_pi/*.pyc
|
*.pyc
|
||||||
|
.ccls-cache
|
||||||
|
compile_commands.json
|
||||||
|
|||||||
@ -1,21 +1,3 @@
|
|||||||
cmake_minimum_required(VERSION 2.8.3)
|
cmake_minimum_required(VERSION 3.5)
|
||||||
project(qt_pi)
|
ADD_SUBDIRECTORY( qt_pi )
|
||||||
find_package(catkin REQUIRED)
|
ADD_SUBDIRECTORY( gazebo_sim )
|
||||||
|
|
||||||
catkin_python_setup()
|
|
||||||
catkin_package(DEPENDS)
|
|
||||||
|
|
||||||
include_directories(
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY config
|
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY launch
|
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY nodes
|
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
)
|
|
||||||
|
|||||||
8
gazebo_sim/CMakeLists.txt
Normal file
8
gazebo_sim/CMakeLists.txt
Normal file
@ -0,0 +1,8 @@
|
|||||||
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
|
project(qt_pi_gazebo_sim)
|
||||||
|
|
||||||
|
set(CMAKE_CXX_FLAGS "-fpermissive -std=c++0x")
|
||||||
|
|
||||||
|
find_package(catkin REQUIRED)
|
||||||
|
|
||||||
|
catkin_package(DEPENDS)
|
||||||
9
gazebo_sim/config/costmap_common_params.yaml
Normal file
9
gazebo_sim/config/costmap_common_params.yaml
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
obstacle_range: 100
|
||||||
|
raytrace_range: 100
|
||||||
|
footprint: [[0.2, 0.3], [0.2, -0.3], [-0.2, -0.3], [-0.2, 0.3]]
|
||||||
|
#robot_radius: ir_of_robot
|
||||||
|
inflation_radius: 0.5
|
||||||
|
|
||||||
|
observation_sources: laser_scan_kinect
|
||||||
|
|
||||||
|
laser_scan_kinect: {sensor_frame: qt_pi/laser, data_type: LaserScan, topic: /qt_pi/scan, marking: true, clearing: true}
|
||||||
8
gazebo_sim/config/global_costmap_params.yaml
Normal file
8
gazebo_sim/config/global_costmap_params.yaml
Normal file
@ -0,0 +1,8 @@
|
|||||||
|
global_costmap:
|
||||||
|
global_frame: map
|
||||||
|
robot_base_frame: qt_pi/base_link
|
||||||
|
update_frequency: 5.0
|
||||||
|
static_map: false
|
||||||
|
rolling_window: true
|
||||||
|
width: 100
|
||||||
|
height: 100
|
||||||
10
gazebo_sim/config/local_costmap_params.yaml
Normal file
10
gazebo_sim/config/local_costmap_params.yaml
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
local_costmap:
|
||||||
|
global_frame: qt_pi/odom
|
||||||
|
robot_base_frame: qt_pi/base_link
|
||||||
|
update_frequency: 5.0
|
||||||
|
publish_frequency: 2.0
|
||||||
|
static_map: false
|
||||||
|
rolling_window: true
|
||||||
|
width: 15.0
|
||||||
|
height: 15.0
|
||||||
|
resolution: 0.05
|
||||||
7
gazebo_sim/launch/depth_to_laser.launch
Normal file
7
gazebo_sim/launch/depth_to_laser.launch
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
<launch>
|
||||||
|
<remap from="image" to="qt_pi/camera_ir/depth/image_raw"/>
|
||||||
|
<node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
|
||||||
|
<param name="scan_height" value="1"/>
|
||||||
|
<param name="range_max" value="100"/>
|
||||||
|
</node>
|
||||||
|
</launch>
|
||||||
9
gazebo_sim/launch/gazebo_sim.launch
Normal file
9
gazebo_sim/launch/gazebo_sim.launch
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
<launch>
|
||||||
|
|
||||||
|
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||||
|
<arg name="world_name" value="$(find qt_pi_gazebo_sim)/worlds/qt_pi_laser.world"/>
|
||||||
|
</include>
|
||||||
|
<!-- <include file="$(find qt_pi_gazebo_sim)/launch/depth_to_laser.launch"/> -->
|
||||||
|
<include file="$(find qt_pi_gazebo_sim)/launch/static_tf.launch"/>
|
||||||
|
<include file="$(find qt_pi_gazebo_sim)/launch/move_base_amcl.launch"/>
|
||||||
|
</launch>
|
||||||
10
gazebo_sim/launch/gmapping.launch
Normal file
10
gazebo_sim/launch/gmapping.launch
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
<launch>
|
||||||
|
<remap from="scan" to="qt_pi/scan"/>
|
||||||
|
<node name="gmapping" pkg="gmapping" type="slam_gmapping">
|
||||||
|
<param name="base_frame" value="qt_pi/base_link" type="string"/>
|
||||||
|
<param name="odom_frame" value="qt_pi/odom" type="string"/>
|
||||||
|
|
||||||
|
<param name="maxUrange" value="9"/>
|
||||||
|
<param name="maxRange" value="10"/>
|
||||||
|
</node>
|
||||||
|
</launch>
|
||||||
23
gazebo_sim/launch/move_base_amcl.launch
Normal file
23
gazebo_sim/launch/move_base_amcl.launch
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
<launch>
|
||||||
|
<master auto="start"/>
|
||||||
|
<remap from="scan" to="qt_pi/scan"/>
|
||||||
|
<remap from="cmd_vel" to="qt_pi/cmd_vel"/>
|
||||||
|
<remap from="odom" to="qt_pi/odom"/>
|
||||||
|
<node name="map_server" pkg="map_server" type="map_server" args="$(find qt_pi_gazebo_sim)/map/playground.yaml"/>
|
||||||
|
|
||||||
|
<node name="amcl" pkg="amcl" type="amcl">
|
||||||
|
|
||||||
|
<param name="base_frame_id" value="qt_pi/base_link" type="string"/>
|
||||||
|
<param name="odom_frame_id" value="qt_pi/odom" type="string"/>
|
||||||
|
<param name="odom_model_type" value="diff" type="string"/>
|
||||||
|
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/>
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/costmap_common_params.yaml" command="load" ns="local_costmap"/>
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/local_costmap_params.yaml" command="load"/>
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/global_costmap_params.yaml" command="load"/>
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/base_local_planner_params.yaml" command="load"/>
|
||||||
|
</node>
|
||||||
|
</launch>
|
||||||
22
gazebo_sim/launch/move_base_gmapping.launch
Normal file
22
gazebo_sim/launch/move_base_gmapping.launch
Normal file
@ -0,0 +1,22 @@
|
|||||||
|
<launch>
|
||||||
|
<remap from="scan" to="qt_pi/scan"/>
|
||||||
|
<remap from="cmd_vel" to="qt_pi/cmd_vel"/>
|
||||||
|
<remap from="odom" to="qt_pi/odom"/>
|
||||||
|
<include file="$(find qt_pi_gazebo_sim)/launch/gmapping.launch"/>
|
||||||
|
|
||||||
|
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
|
||||||
|
<param name="footprint_padding" value="0.01"/>
|
||||||
|
<param name="controller_frequency" value="10.0"/>
|
||||||
|
<param name="controller_patience" value="3.0"/>
|
||||||
|
|
||||||
|
<param name="oscillation_timeout" value="30.0"/>
|
||||||
|
<param name="oscillation_distance" value="0.5"/>
|
||||||
|
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/>
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/costmap_common_params.yaml" command="load" ns="local_costmap"/>
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/local_costmap_params.yaml" command="load"/>
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/global_costmap_params.yaml" command="load"/>
|
||||||
|
<rosparam file="$(find qt_pi_gazebo_sim)/config/base_local_planner_params.yaml" command="load"/>
|
||||||
|
|
||||||
|
</node>
|
||||||
|
</launch>
|
||||||
3
gazebo_sim/launch/static_tf.launch
Normal file
3
gazebo_sim/launch/static_tf.launch
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
<launch>
|
||||||
|
<node pkg="tf" type="static_transform_publisher" name="laser_frame" args="0.2 0 0.2 0 0 0 1 qt_pi/base_link qt_pi/laser 33"/>
|
||||||
|
</launch>
|
||||||
4
gazebo_sim/launch/teleop.launch
Normal file
4
gazebo_sim/launch/teleop.launch
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
<launch>
|
||||||
|
<remap from="cmd_vel" to="qt_pi/cmd_vel"/>
|
||||||
|
<node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" respawn="false" name="qt_pi-teleop" output="screen"></node>
|
||||||
|
</launch>
|
||||||
8
gazebo_sim/map/frames.gv
Normal file
8
gazebo_sim/map/frames.gv
Normal file
@ -0,0 +1,8 @@
|
|||||||
|
digraph G {
|
||||||
|
"qt_pi/odom" -> "qt_pi/base_link"[label="Broadcaster: /gazebo\nAverage rate: 30.199 Hz\nMost recent transform: 604.007 ( 0.018 sec old)\nBuffer length: 4.967 sec\n"];
|
||||||
|
"qt_pi/base_link" -> "qt_pi/laser"[label="Broadcaster: /laser_frame\nAverage rate: 30.419 Hz\nMost recent transform: 604.039 ( -0.014 sec old)\nBuffer length: 4.964 sec\n"];
|
||||||
|
edge [style=invis];
|
||||||
|
subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
|
||||||
|
"Recorded at time: 604.025"[ shape=plaintext ] ;
|
||||||
|
}->"qt_pi/odom";
|
||||||
|
}
|
||||||
BIN
gazebo_sim/map/frames.pdf
Normal file
BIN
gazebo_sim/map/frames.pdf
Normal file
Binary file not shown.
BIN
gazebo_sim/map/playground.pgm
Normal file
BIN
gazebo_sim/map/playground.pgm
Normal file
Binary file not shown.
6
gazebo_sim/map/playground.yaml
Normal file
6
gazebo_sim/map/playground.yaml
Normal file
@ -0,0 +1,6 @@
|
|||||||
|
image: playground.pgm
|
||||||
|
resolution: 0.050000
|
||||||
|
origin: [-4.000000, -13.000000, 0.000000]
|
||||||
|
negate: 0
|
||||||
|
occupied_thresh: 0.65
|
||||||
|
free_thresh: 0.196
|
||||||
17
gazebo_sim/package.xml
Normal file
17
gazebo_sim/package.xml
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
<package format="2">
|
||||||
|
<name>qt_pi_gazebo_sim</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>The qt_pi_gazebo_sim package</description>
|
||||||
|
<maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
|
||||||
|
<license>MIT</license>
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<exec_depend>gmapping</exec_depend>
|
||||||
|
<exec_depend>amcl</exec_depend>
|
||||||
|
<exec_depend>move_base</exec_depend>
|
||||||
|
<exec_depend>depthimage_to_laserscan</exec_depend>
|
||||||
|
<exec_depend>map_server</exec_depend>
|
||||||
|
<exec_depend>static_transform_publisher</exec_depend>
|
||||||
|
<exec_depend>teleop_twist_keyboard</exec_depend>
|
||||||
|
<export>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
2046
gazebo_sim/worlds/playground.world
Normal file
2046
gazebo_sim/worlds/playground.world
Normal file
File diff suppressed because it is too large
Load Diff
2061
gazebo_sim/worlds/qt_pi_laser.world
Normal file
2061
gazebo_sim/worlds/qt_pi_laser.world
Normal file
File diff suppressed because it is too large
Load Diff
@ -1,9 +0,0 @@
|
|||||||
#include <errno.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <pthread.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <termios.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include "commands.h"
|
|
||||||
#include "payload_specs.h"
|
|
||||||
17
package.xml
17
package.xml
@ -1,17 +0,0 @@
|
|||||||
<package format="2">
|
|
||||||
<name>qt_pi</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>The qt_pi package</description>
|
|
||||||
<maintainer email="rishabhkundu13@gmail.com">Rishabh Kundu</maintainer>
|
|
||||||
<license>MIT</license>
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
|
|
||||||
<exec_depend>rospy</exec_depend>
|
|
||||||
<exec_depend>std_msgs</exec_depend>
|
|
||||||
<exec_depend>geometry_msgs</exec_depend>
|
|
||||||
<exec_depend>nav_msgs</exec_depend>
|
|
||||||
<exec_depend>tf</exec_depend>
|
|
||||||
<exec_depend>python-serial</exec_depend>
|
|
||||||
<export>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
68
qt_pi/CMakeLists.txt
Normal file
68
qt_pi/CMakeLists.txt
Normal file
@ -0,0 +1,68 @@
|
|||||||
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
|
project(qt_pi)
|
||||||
|
|
||||||
|
set(CMAKE_CXX_FLAGS "-std=c++0x")
|
||||||
|
#-fpermissive
|
||||||
|
#ros setup
|
||||||
|
find_package(catkin REQUIRED COMPONENTS
|
||||||
|
qt_build
|
||||||
|
roscpp
|
||||||
|
roslib
|
||||||
|
)
|
||||||
|
|
||||||
|
catkin_python_setup()
|
||||||
|
catkin_package(DEPENDS)
|
||||||
|
|
||||||
|
#Qt setup and MOC generation
|
||||||
|
find_package(Qt5 REQUIRED COMPONENTS Core Gui OpenGL)
|
||||||
|
set(MOC include/speller/speller.h include/mapview/mapview.h)
|
||||||
|
set(HPP include/mapview/vertex.h)
|
||||||
|
set(SRCS src/speller.cpp src/mapview.cpp )
|
||||||
|
set(CMAKE_INCLUDE_CURRENT_DIR ON)
|
||||||
|
qt5_wrap_cpp(MOC_HPP ${MOC})
|
||||||
|
qt5_add_resources(RCC resources/resources.qrc)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#Mcrypt library includes
|
||||||
|
find_package(PkgConfig REQUIRED)
|
||||||
|
find_path(Mcrypt_INCLUDE_DIR mcrypt.h PATHS /usr/include)
|
||||||
|
set(Mcrypt_LIB_PATHS /usr/lib)
|
||||||
|
find_library(Mcrypt_LIBS NAMES mcrypt rtfilter PATHS ${Mcrypt_LIB_PATHS})
|
||||||
|
include_directories(${Mcrypt_INCLUDE_DIRS})
|
||||||
|
link_directories(${Mcrypt_LIBS})
|
||||||
|
|
||||||
|
#Gattlib includes
|
||||||
|
pkg_search_module(GATTLIB REQUIRED gattlib)
|
||||||
|
pkg_search_module(GLIB REQUIRED glib-2.0)
|
||||||
|
include_directories(${GLIB_INCLUDE_DIRS})
|
||||||
|
|
||||||
|
|
||||||
|
include_directories(include
|
||||||
|
${catkin_INCLUDE_DIRS}
|
||||||
|
)
|
||||||
|
|
||||||
|
#ble_connect library generation
|
||||||
|
add_library(ble_connect include/ble_connect/ble_connect.h src/ble_connect.c)
|
||||||
|
target_link_libraries(ble_connect ${GATTLIB_LIBRARIES} ${GATTLIB_LDFLAGS} ${GLIB_LDFLAGS} pthread ${Mcrypt_LIBS})
|
||||||
|
|
||||||
|
#speller library generation
|
||||||
|
add_library(speller ${SRCS} ${FORMS_HPP} ${MOC_HPP} ${RCC} ${HPP})
|
||||||
|
target_link_libraries(speller Qt5::Widgets Qt5::Core Qt5::OpenGL Qt5::Gui ${catkin_LIBRARIES} ble_connect)
|
||||||
|
|
||||||
|
|
||||||
|
#ble_connect_test executable generation
|
||||||
|
add_executable(ble_connect_test tests/ble_test.cpp)
|
||||||
|
target_link_libraries(ble_connect_test ble_connect)
|
||||||
|
|
||||||
|
#QImage view test executable generation
|
||||||
|
add_executable(map_load_test tests/map_load_test.cpp)
|
||||||
|
target_link_libraries(map_load_test speller)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#main executable generation
|
||||||
|
add_executable(speller_ui src/main.cpp)
|
||||||
|
target_link_libraries(speller_ui speller)
|
||||||
7
qt_pi/config/70-emotiv.rules
Normal file
7
qt_pi/config/70-emotiv.rules
Normal file
@ -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"
|
||||||
11
qt_pi/config/base_local_planner_params.yaml
Normal file
11
qt_pi/config/base_local_planner_params.yaml
Normal file
@ -0,0 +1,11 @@
|
|||||||
|
TrajectoryPlannerROS:
|
||||||
|
max_vel_x: 0.45
|
||||||
|
min_vel_x: 0.1
|
||||||
|
max_vel_theta: 1.0
|
||||||
|
min_in_place_vel_theta: 0.4
|
||||||
|
|
||||||
|
acc_lim_theta: 3.2
|
||||||
|
acc_lim_x: 2.5
|
||||||
|
acc_lim_y: 2.5
|
||||||
|
|
||||||
|
holonomic_robot: true
|
||||||
80
qt_pi/doc/Connecting on debian.md
Normal file
80
qt_pi/doc/Connecting on debian.md
Normal file
@ -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]`
|
||||||
33
qt_pi/include/ble_connect/ble_connect.h
Normal file
33
qt_pi/include/ble_connect/ble_connect.h
Normal file
@ -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;
|
||||||
|
};
|
||||||
|
#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 int 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
|
||||||
71
qt_pi/include/mapview/mapview.h
Normal file
71
qt_pi/include/mapview/mapview.h
Normal file
@ -0,0 +1,71 @@
|
|||||||
|
#ifndef MAPVIEW
|
||||||
|
#define MAPVIEW
|
||||||
|
|
||||||
|
#include "vertex.h"
|
||||||
|
|
||||||
|
#include <QDebug>
|
||||||
|
#include <QOpenGLBuffer>
|
||||||
|
#include <QOpenGLFunctions_3_1>
|
||||||
|
#include <QOpenGLShaderProgram>
|
||||||
|
#include <QOpenGLTexture>
|
||||||
|
#include <QOpenGLVertexArrayObject>
|
||||||
|
#include <QOpenGLWindow>
|
||||||
|
#include <QString>
|
||||||
|
#include <chrono>
|
||||||
|
#include <thread>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
class QOpenGLShaderProgram;
|
||||||
|
|
||||||
|
class MapView : public QOpenGLWindow, protected QOpenGLFunctions_3_1 {
|
||||||
|
Q_OBJECT
|
||||||
|
public:
|
||||||
|
explicit MapView(QWidget *parent = 0);
|
||||||
|
~MapView();
|
||||||
|
|
||||||
|
void start_blinking();
|
||||||
|
void stop_blinking();
|
||||||
|
void toggle_blinking();
|
||||||
|
|
||||||
|
protected:
|
||||||
|
GLsizei count;
|
||||||
|
GLint startingElements[4] = {0, 4, 8, 12};
|
||||||
|
GLsizei counts[4] = {4, 4, 4, 4};
|
||||||
|
|
||||||
|
void initializeGL() override;
|
||||||
|
void paintGL() override;
|
||||||
|
void resizeGL(int, int) override;
|
||||||
|
void teardownGL();
|
||||||
|
void printContextInformation();
|
||||||
|
|
||||||
|
private:
|
||||||
|
QOpenGLBuffer m_vertex;
|
||||||
|
QOpenGLVertexArrayObject m_object;
|
||||||
|
QOpenGLShaderProgram *m_program;
|
||||||
|
|
||||||
|
QOpenGLBuffer map_vertex;
|
||||||
|
QOpenGLVertexArrayObject map_object;
|
||||||
|
QOpenGLShaderProgram *map_program;
|
||||||
|
QOpenGLTexture *map_texture;
|
||||||
|
Vertex *vertices;
|
||||||
|
int vertices_len;
|
||||||
|
|
||||||
|
volatile int sleep_control = 1;
|
||||||
|
volatile int blinking;
|
||||||
|
int map_redraw;
|
||||||
|
|
||||||
|
std::thread *t;
|
||||||
|
|
||||||
|
void run();
|
||||||
|
void keyPressEvent(QKeyEvent *key) override;
|
||||||
|
void mousePressEvent(QMouseEvent *event) override;
|
||||||
|
signals:
|
||||||
|
void update_GUI();
|
||||||
|
void keyPress(QKeyEvent *key);
|
||||||
|
void map_loaded();
|
||||||
|
void mouse_press(QMouseEvent *);
|
||||||
|
private slots:
|
||||||
|
void load_texture(int, Vertex *, QImage *);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
88
qt_pi/include/mapview/vertex.h
Normal file
88
qt_pi/include/mapview/vertex.h
Normal file
@ -0,0 +1,88 @@
|
|||||||
|
#ifndef VERTEX_H
|
||||||
|
#define VERTEX_H
|
||||||
|
|
||||||
|
#include <QVector2D>
|
||||||
|
#include <QVector3D>
|
||||||
|
#include <QVector4D>
|
||||||
|
|
||||||
|
class Vertex {
|
||||||
|
public:
|
||||||
|
// Constructors
|
||||||
|
Q_DECL_CONSTEXPR Vertex();
|
||||||
|
Q_DECL_CONSTEXPR explicit Vertex(const QVector3D &position);
|
||||||
|
Q_DECL_CONSTEXPR Vertex(const QVector3D &position, const QVector4D &color);
|
||||||
|
Q_DECL_CONSTEXPR Vertex(const QVector3D &position, const QVector4D &color,
|
||||||
|
const QVector2D &texCoord);
|
||||||
|
|
||||||
|
// Accessors / Mutators
|
||||||
|
Q_DECL_CONSTEXPR const QVector3D &position() const;
|
||||||
|
Q_DECL_CONSTEXPR const QVector4D &color() const;
|
||||||
|
Q_DECL_CONSTEXPR const QVector2D &texCoord() const;
|
||||||
|
void setPosition(const QVector3D &position);
|
||||||
|
void setColor(const QVector4D &color);
|
||||||
|
void setTexCoord(const QVector2D &texCoord);
|
||||||
|
|
||||||
|
// OpenGL Helpers
|
||||||
|
static const int PositionTupleSize = 3;
|
||||||
|
static const int ColorTupleSize = 4;
|
||||||
|
static const int TexCoordTupleSize = 2;
|
||||||
|
static Q_DECL_CONSTEXPR int positionOffset();
|
||||||
|
static Q_DECL_CONSTEXPR int colorOffset();
|
||||||
|
static Q_DECL_CONSTEXPR int texCoordOffset();
|
||||||
|
static Q_DECL_CONSTEXPR int stride();
|
||||||
|
|
||||||
|
private:
|
||||||
|
QVector3D m_position;
|
||||||
|
QVector4D m_color;
|
||||||
|
QVector2D m_texCoord;
|
||||||
|
};
|
||||||
|
|
||||||
|
/*******************************************************************************
|
||||||
|
* Inline Implementation
|
||||||
|
******************************************************************************/
|
||||||
|
|
||||||
|
// Note: Q_MOVABLE_TYPE means it can be memcpy'd.
|
||||||
|
Q_DECLARE_TYPEINFO(Vertex, Q_MOVABLE_TYPE);
|
||||||
|
|
||||||
|
// Constructors
|
||||||
|
Q_DECL_CONSTEXPR inline Vertex::Vertex() {}
|
||||||
|
Q_DECL_CONSTEXPR inline Vertex::Vertex(const QVector3D &position)
|
||||||
|
: m_position(position) {}
|
||||||
|
Q_DECL_CONSTEXPR inline Vertex::Vertex(const QVector3D &position,
|
||||||
|
const QVector4D &color)
|
||||||
|
: m_position(position), m_color(color) {}
|
||||||
|
Q_DECL_CONSTEXPR inline Vertex::Vertex(const QVector3D &position,
|
||||||
|
const QVector4D &color,
|
||||||
|
const QVector2D &texCoord)
|
||||||
|
: m_position(position), m_color(color), m_texCoord(texCoord) {}
|
||||||
|
|
||||||
|
// Accessors / Mutators
|
||||||
|
Q_DECL_CONSTEXPR inline const QVector3D &Vertex::position() const {
|
||||||
|
return m_position;
|
||||||
|
}
|
||||||
|
Q_DECL_CONSTEXPR inline const QVector4D &Vertex::color() const {
|
||||||
|
return m_color;
|
||||||
|
}
|
||||||
|
Q_DECL_CONSTEXPR inline const QVector2D &Vertex::texCoord() const {
|
||||||
|
return m_texCoord;
|
||||||
|
}
|
||||||
|
void inline Vertex::setPosition(const QVector3D &position) {
|
||||||
|
m_position = position;
|
||||||
|
}
|
||||||
|
void inline Vertex::setColor(const QVector4D &color) { m_color = color; }
|
||||||
|
void inline Vertex::setTexCoord(const QVector2D &texCoord) {
|
||||||
|
m_texCoord = texCoord;
|
||||||
|
}
|
||||||
|
// OpenGL Helpers
|
||||||
|
Q_DECL_CONSTEXPR inline int Vertex::positionOffset() {
|
||||||
|
return offsetof(Vertex, m_position);
|
||||||
|
}
|
||||||
|
Q_DECL_CONSTEXPR inline int Vertex::colorOffset() {
|
||||||
|
return offsetof(Vertex, m_color);
|
||||||
|
}
|
||||||
|
Q_DECL_CONSTEXPR inline int Vertex::texCoordOffset() {
|
||||||
|
return offsetof(Vertex, m_texCoord);
|
||||||
|
}
|
||||||
|
Q_DECL_CONSTEXPR inline int Vertex::stride() { return sizeof(Vertex); }
|
||||||
|
|
||||||
|
#endif // VERTEX_H
|
||||||
72
qt_pi/include/speller/speller.h
Normal file
72
qt_pi/include/speller/speller.h
Normal file
@ -0,0 +1,72 @@
|
|||||||
|
#ifndef SPELLER
|
||||||
|
#define SPELLER
|
||||||
|
|
||||||
|
#include "ble_connect/ble_connect.h"
|
||||||
|
#include "mapview/mapview.h"
|
||||||
|
#include <QKeyEvent>
|
||||||
|
#include <QThread>
|
||||||
|
#include <geometry_msgs/Point.h>
|
||||||
|
#include <geometry_msgs/Pose.h>
|
||||||
|
#include <geometry_msgs/PoseStamped.h>
|
||||||
|
#include <geometry_msgs/Quaternion.h>
|
||||||
|
#include <map>
|
||||||
|
#include <nav_msgs/GetMap.h>
|
||||||
|
#include <nav_msgs/OccupancyGrid.h>
|
||||||
|
#include <nav_msgs/Odometry.h>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
class Speller : public QThread {
|
||||||
|
Q_OBJECT
|
||||||
|
public:
|
||||||
|
explicit Speller(int argc, char **argv);
|
||||||
|
~Speller();
|
||||||
|
const std::map<float, int> frequency = {
|
||||||
|
{4.286f, 14}, {5.0f, 12}, {6.0f, 10}, {7.5f, 8}};
|
||||||
|
MapView *window;
|
||||||
|
|
||||||
|
private:
|
||||||
|
int argc;
|
||||||
|
char **argv;
|
||||||
|
void run() override;
|
||||||
|
|
||||||
|
enum Map_Shape { SQUARE, HORIZONTAL, VERTICAL };
|
||||||
|
enum Classify_Type { TOP_RIGHT = 1, TOP_LEFT, BOTTOM_LEFT, BOTTOM_RIGHT };
|
||||||
|
|
||||||
|
QImage *curr_map;
|
||||||
|
QRect *area_displayed;
|
||||||
|
uint8_t *map_tex_buffer;
|
||||||
|
int map_shape;
|
||||||
|
int classifier_result;
|
||||||
|
|
||||||
|
ros::NodeHandle *nh;
|
||||||
|
ros::Publisher pub_goal;
|
||||||
|
ros::Subscriber sub_pose;
|
||||||
|
ros::ServiceClient map_client;
|
||||||
|
|
||||||
|
nav_msgs::OccupancyGrid map;
|
||||||
|
geometry_msgs::PoseStamped pose;
|
||||||
|
geometry_msgs::Pose pose_current;
|
||||||
|
|
||||||
|
struct insight_data *buf;
|
||||||
|
int buf_len = 100;
|
||||||
|
|
||||||
|
volatile int map_loading_done;
|
||||||
|
|
||||||
|
volatile int spelling;
|
||||||
|
void reconfigure_map(int, int);
|
||||||
|
void poseCB(nav_msgs::Odometry::ConstPtr &msg);
|
||||||
|
__inline__ void init_ble();
|
||||||
|
__inline__ void get_ble_data();
|
||||||
|
__inline__ void init_ros();
|
||||||
|
__inline__ void load_map();
|
||||||
|
|
||||||
|
void classify(int len, struct insight_data *buf);
|
||||||
|
QPoint me;
|
||||||
|
private slots:
|
||||||
|
void keyEvent(QKeyEvent *key);
|
||||||
|
void map_loaded();
|
||||||
|
void mouse_press(QMouseEvent *);
|
||||||
|
signals:
|
||||||
|
void load_texture(int, Vertex *, QImage *);
|
||||||
|
};
|
||||||
|
#endif
|
||||||
4
qt_pi/msg/EEG_data.msg
Normal file
4
qt_pi/msg/EEG_data.msg
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
int channel[5]
|
||||||
|
float32 data[5]
|
||||||
|
int cq[5]
|
||||||
|
int batt_lvl
|
||||||
18
qt_pi/package.xml
Normal file
18
qt_pi/package.xml
Normal file
@ -0,0 +1,18 @@
|
|||||||
|
<package format="2">
|
||||||
|
<name>qt_pi</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>The qt_pi package</description>
|
||||||
|
<maintainer email="rishabhkundu13@gmail.com">Rishabh Kundu</maintainer>
|
||||||
|
<license>MIT</license>
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<build_depend>qt_build</build_depend>
|
||||||
|
<build_depend>roscpp</build_depend>
|
||||||
|
<build_depend>roslib</build_depend>
|
||||||
|
|
||||||
|
<exec_depend>rospy</exec_depend>
|
||||||
|
<exec_depend>geometry_msgs</exec_depend>
|
||||||
|
<exec_depend>nav_msgs</exec_depend>
|
||||||
|
<exec_depend>tf</exec_depend>
|
||||||
|
<exec_depend>python-serial</exec_depend>
|
||||||
|
<export></export>
|
||||||
|
</package>
|
||||||
8
qt_pi/resources/resources.qrc
Normal file
8
qt_pi/resources/resources.qrc
Normal file
@ -0,0 +1,8 @@
|
|||||||
|
<RCC>
|
||||||
|
<qresource prefix="/">
|
||||||
|
<file>shaders/blink.frag</file>
|
||||||
|
<file>shaders/blink.vert</file>
|
||||||
|
<file>shaders/map.vert</file>
|
||||||
|
<file>shaders/map.frag</file>
|
||||||
|
</qresource>
|
||||||
|
</RCC>
|
||||||
8
qt_pi/resources/shaders/blink.frag
Normal file
8
qt_pi/resources/shaders/blink.frag
Normal file
@ -0,0 +1,8 @@
|
|||||||
|
#version 330
|
||||||
|
in vec4 vColor;
|
||||||
|
out vec4 fColor;
|
||||||
|
|
||||||
|
void main()
|
||||||
|
{
|
||||||
|
fColor = vColor;
|
||||||
|
}
|
||||||
10
qt_pi/resources/shaders/blink.vert
Normal file
10
qt_pi/resources/shaders/blink.vert
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
#version 330
|
||||||
|
layout(location = 0) in vec3 position;
|
||||||
|
layout(location = 1) in vec4 color;
|
||||||
|
out vec4 vColor;
|
||||||
|
|
||||||
|
void main()
|
||||||
|
{
|
||||||
|
gl_Position = vec4(position, 1.0);
|
||||||
|
vColor = color;
|
||||||
|
}
|
||||||
13
qt_pi/resources/shaders/map.frag
Normal file
13
qt_pi/resources/shaders/map.frag
Normal file
@ -0,0 +1,13 @@
|
|||||||
|
#version 330 core
|
||||||
|
out vec4 FragColor;
|
||||||
|
|
||||||
|
in vec4 ourColor;
|
||||||
|
in vec2 TexCoord;
|
||||||
|
|
||||||
|
uniform sampler2D ourTexture;
|
||||||
|
|
||||||
|
void main()
|
||||||
|
{
|
||||||
|
//FragColor = ourColor;
|
||||||
|
FragColor = texture(ourTexture, TexCoord);
|
||||||
|
}
|
||||||
14
qt_pi/resources/shaders/map.vert
Normal file
14
qt_pi/resources/shaders/map.vert
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
#version 330 core
|
||||||
|
layout (location = 0) in vec3 aPos;
|
||||||
|
layout (location = 1) in vec3 aColor;
|
||||||
|
layout (location = 2) in vec2 aTexCoord;
|
||||||
|
|
||||||
|
out vec4 ourColor;
|
||||||
|
out vec2 TexCoord;
|
||||||
|
|
||||||
|
void main()
|
||||||
|
{
|
||||||
|
gl_Position = vec4(aPos, 1.0);
|
||||||
|
ourColor = vec4(aColor, 1.0);
|
||||||
|
TexCoord = aTexCoord;
|
||||||
|
}
|
||||||
163
qt_pi/src/ble_connect.c
Normal file
163
qt_pi/src/ble_connect.c
Normal file
@ -0,0 +1,163 @@
|
|||||||
|
#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;
|
||||||
|
volatile 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
int insight_write_to_buffer(int len, struct insight_data *buf) {
|
||||||
|
if (status != 2)
|
||||||
|
return status;
|
||||||
|
data_buffer = buf;
|
||||||
|
data_buffer_length = len;
|
||||||
|
data_index = 0;
|
||||||
|
write_to_buffer = 1;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
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; }
|
||||||
22
qt_pi/src/main.cpp
Normal file
22
qt_pi/src/main.cpp
Normal file
@ -0,0 +1,22 @@
|
|||||||
|
#include "speller/speller.h"
|
||||||
|
#include <QGuiApplication>
|
||||||
|
#include <QSurfaceFormat>
|
||||||
|
|
||||||
|
int main(int argc, char *argv[]) {
|
||||||
|
|
||||||
|
QGuiApplication app(argc, argv);
|
||||||
|
|
||||||
|
QSurfaceFormat format;
|
||||||
|
format.setSwapInterval(1);
|
||||||
|
format.setRenderableType(QSurfaceFormat::OpenGL);
|
||||||
|
format.setProfile(QSurfaceFormat::CoreProfile);
|
||||||
|
format.setVersion(3, 3);
|
||||||
|
|
||||||
|
// Set the window up
|
||||||
|
Speller spell(argc, argv);
|
||||||
|
spell.window->setFormat(format);
|
||||||
|
spell.window->show();
|
||||||
|
spell.start();
|
||||||
|
|
||||||
|
return app.exec();
|
||||||
|
}
|
||||||
300
qt_pi/src/mapview.cpp
Normal file
300
qt_pi/src/mapview.cpp
Normal file
@ -0,0 +1,300 @@
|
|||||||
|
#include "mapview/mapview.h"
|
||||||
|
|
||||||
|
MapView::MapView(QWidget *parent) {
|
||||||
|
connect(this, SIGNAL(update_GUI()), this, SLOT(update()));
|
||||||
|
|
||||||
|
vertices_len = 144;
|
||||||
|
vertices = (Vertex *)malloc(vertices_len);
|
||||||
|
memset(vertices, 0, vertices_len);
|
||||||
|
|
||||||
|
map_texture = NULL;
|
||||||
|
blinking = 0;
|
||||||
|
count = 0;
|
||||||
|
map_redraw = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
MapView::~MapView() {
|
||||||
|
stop_blinking();
|
||||||
|
makeCurrent();
|
||||||
|
teardownGL();
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::run() {
|
||||||
|
usleep(500000);
|
||||||
|
|
||||||
|
uint fps = 0;
|
||||||
|
uint sq_state = 0;
|
||||||
|
uint pre_sq_state = 0;
|
||||||
|
|
||||||
|
while (blinking) {
|
||||||
|
|
||||||
|
auto time = std::chrono::high_resolution_clock::now();
|
||||||
|
|
||||||
|
count = 0;
|
||||||
|
int changed = 0;
|
||||||
|
|
||||||
|
fps++;
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
if (!(fps % (i + 4))) {
|
||||||
|
sq_state = sq_state ^ (0b1 << i);
|
||||||
|
changed = 1;
|
||||||
|
if (((sq_state >> i) & 0b1))
|
||||||
|
startingElements[count++] = i * 4;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (changed) {
|
||||||
|
|
||||||
|
sleep_control = 0;
|
||||||
|
emit(update_GUI());
|
||||||
|
while (!sleep_control)
|
||||||
|
sleep(0);
|
||||||
|
|
||||||
|
pre_sq_state = sq_state;
|
||||||
|
}
|
||||||
|
|
||||||
|
usleep(16665 - std::chrono::duration_cast<std::chrono::microseconds>(
|
||||||
|
std::chrono::high_resolution_clock::now() - time)
|
||||||
|
.count());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::stop_blinking() {
|
||||||
|
if (!blinking)
|
||||||
|
return;
|
||||||
|
blinking = 0;
|
||||||
|
count = 0;
|
||||||
|
sleep_control = 1;
|
||||||
|
t->join();
|
||||||
|
free(t);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::toggle_blinking() {
|
||||||
|
if (blinking)
|
||||||
|
stop_blinking();
|
||||||
|
else
|
||||||
|
start_blinking();
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::start_blinking() {
|
||||||
|
|
||||||
|
if (blinking)
|
||||||
|
return;
|
||||||
|
blinking = 1;
|
||||||
|
t = new std::thread(&MapView::run, this);
|
||||||
|
sleep(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
static const Vertex sg_vertexes[] = {
|
||||||
|
Vertex(QVector3D(-0.55f, 0.55f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(-0.45f, 0.55f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(-0.45f, 0.45f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(-0.55f, 0.45f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
|
||||||
|
Vertex(QVector3D(-0.55f, -0.55f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(-0.45f, -0.55f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(-0.45f, -0.45f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(-0.55f, -0.45f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
|
||||||
|
Vertex(QVector3D(0.55f, 0.55f, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(0.45f, 0.55f, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(0.45f, 0.45f, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(0.55f, 0.45f, 0.0f), QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
|
||||||
|
Vertex(QVector3D(0.55f, -0.55f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(0.45f, -0.55f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(0.45f, -0.45f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
Vertex(QVector3D(0.55f, -0.45f, 0.0f),
|
||||||
|
QVector4D(0.75f, 0.75f, 0.75f, 1.0f)),
|
||||||
|
|
||||||
|
Vertex(QVector3D(-0.55f, 0.55f, 0.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(-0.45f, 0.55f, 0.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(-0.45f, 0.45f, 0.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(-0.55f, 0.45f, 0.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
|
||||||
|
Vertex(QVector3D(-0.55f, -0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(-0.45f, -0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(-0.45f, -0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(-0.55f, -0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
|
||||||
|
Vertex(QVector3D(0.55f, 0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(0.45f, 0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(0.45f, 0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(0.55f, 0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
|
||||||
|
Vertex(QVector3D(0.55f, -0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(0.45f, -0.55f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(0.45f, -0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(0.55f, -0.45f, 0.0f), QVector4D(0.0f, 0.0f, 0.0f, 0.0f))};
|
||||||
|
|
||||||
|
void MapView::initializeGL() {
|
||||||
|
initializeOpenGLFunctions();
|
||||||
|
printContextInformation();
|
||||||
|
|
||||||
|
glClearColor(0, 0, 0, 1);
|
||||||
|
{
|
||||||
|
// Create Shader (Do not release until VAO is created)
|
||||||
|
map_program = new QOpenGLShaderProgram();
|
||||||
|
map_program->addShaderFromSourceFile(QOpenGLShader::Vertex,
|
||||||
|
":/shaders/map.vert");
|
||||||
|
map_program->addShaderFromSourceFile(QOpenGLShader::Fragment,
|
||||||
|
":/shaders/map.frag");
|
||||||
|
map_program->link();
|
||||||
|
map_program->bind();
|
||||||
|
map_vertex.create();
|
||||||
|
|
||||||
|
map_vertex.bind();
|
||||||
|
map_vertex.setUsagePattern(QOpenGLBuffer::DynamicDraw);
|
||||||
|
map_vertex.allocate(vertices, vertices_len);
|
||||||
|
// Create Vertex Array Object
|
||||||
|
map_object.create();
|
||||||
|
map_object.bind();
|
||||||
|
map_program->enableAttributeArray(0);
|
||||||
|
map_program->enableAttributeArray(1);
|
||||||
|
map_program->enableAttributeArray(2);
|
||||||
|
|
||||||
|
map_program->setAttributeBuffer(0, GL_FLOAT, Vertex::positionOffset(),
|
||||||
|
Vertex::PositionTupleSize,
|
||||||
|
Vertex::stride());
|
||||||
|
map_program->setAttributeBuffer(1, GL_FLOAT, Vertex::colorOffset(),
|
||||||
|
Vertex::ColorTupleSize, Vertex::stride());
|
||||||
|
map_program->setAttributeBuffer(2, GL_FLOAT, Vertex::texCoordOffset(),
|
||||||
|
Vertex::TexCoordTupleSize,
|
||||||
|
Vertex::stride());
|
||||||
|
|
||||||
|
// map_program->setUniformValue("ourTexture", 0);
|
||||||
|
|
||||||
|
map_object.release();
|
||||||
|
map_vertex.release();
|
||||||
|
map_program->release();
|
||||||
|
|
||||||
|
m_program = new QOpenGLShaderProgram();
|
||||||
|
m_program->addShaderFromSourceFile(QOpenGLShader::Vertex,
|
||||||
|
":/shaders/blink.vert");
|
||||||
|
m_program->addShaderFromSourceFile(QOpenGLShader::Fragment,
|
||||||
|
":/shaders/blink.frag");
|
||||||
|
m_program->link();
|
||||||
|
m_program->bind();
|
||||||
|
|
||||||
|
// Create Buffer (Do not release until VAO is created)
|
||||||
|
m_vertex.create();
|
||||||
|
m_vertex.bind();
|
||||||
|
m_vertex.setUsagePattern(QOpenGLBuffer::StaticDraw);
|
||||||
|
m_vertex.allocate(sg_vertexes, sizeof(sg_vertexes));
|
||||||
|
|
||||||
|
// Create Vertex Array Object
|
||||||
|
m_object.create();
|
||||||
|
m_object.bind();
|
||||||
|
m_program->enableAttributeArray(0);
|
||||||
|
m_program->enableAttributeArray(1);
|
||||||
|
m_program->setAttributeBuffer(0, GL_FLOAT, Vertex::positionOffset(),
|
||||||
|
Vertex::PositionTupleSize, Vertex::stride());
|
||||||
|
m_program->setAttributeBuffer(1, GL_FLOAT, Vertex::colorOffset(),
|
||||||
|
Vertex::ColorTupleSize, Vertex::stride());
|
||||||
|
|
||||||
|
// Release (unbind) all
|
||||||
|
m_object.release();
|
||||||
|
m_vertex.release();
|
||||||
|
m_program->release();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::paintGL() {
|
||||||
|
|
||||||
|
glClear(GL_COLOR_BUFFER_BIT);
|
||||||
|
|
||||||
|
if (map_redraw == 1) {
|
||||||
|
map_program->bind();
|
||||||
|
map_object.bind();
|
||||||
|
map_texture->bind();
|
||||||
|
glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
|
||||||
|
map_texture->release();
|
||||||
|
map_object.release();
|
||||||
|
map_program->release();
|
||||||
|
}
|
||||||
|
|
||||||
|
m_program->bind();
|
||||||
|
{
|
||||||
|
m_object.bind();
|
||||||
|
glMultiDrawArrays(GL_TRIANGLE_FAN, startingElements, counts, count);
|
||||||
|
m_object.release();
|
||||||
|
}
|
||||||
|
m_program->release();
|
||||||
|
|
||||||
|
sleep_control = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::resizeGL(int w, int h) {
|
||||||
|
|
||||||
|
glViewport(0, 0, w, h);
|
||||||
|
glMatrixMode(GL_PROJECTION);
|
||||||
|
glLoadIdentity();
|
||||||
|
glMatrixMode(GL_MODELVIEW);
|
||||||
|
glLoadIdentity();
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::printContextInformation() {
|
||||||
|
QString glType;
|
||||||
|
QString glVersion;
|
||||||
|
QString glProfile;
|
||||||
|
|
||||||
|
// Get Version Information
|
||||||
|
glType = (context()->isOpenGLES()) ? "OpenGL ES" : "OpenGL";
|
||||||
|
glVersion = reinterpret_cast<const char *>(glGetString(GL_VERSION));
|
||||||
|
|
||||||
|
// Get Profile Information
|
||||||
|
#define CASE(c) \
|
||||||
|
case QSurfaceFormat::c: \
|
||||||
|
glProfile = #c; \
|
||||||
|
break
|
||||||
|
switch (format().profile()) {
|
||||||
|
CASE(NoProfile);
|
||||||
|
CASE(CoreProfile);
|
||||||
|
CASE(CompatibilityProfile);
|
||||||
|
}
|
||||||
|
#undef CASE
|
||||||
|
|
||||||
|
// qPrintable() will print our QString w/o quotes around it.
|
||||||
|
qDebug() << qPrintable(glType) << qPrintable(glVersion) << "("
|
||||||
|
<< qPrintable(glProfile) << ")";
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::teardownGL() {
|
||||||
|
map_object.destroy();
|
||||||
|
map_vertex.destroy();
|
||||||
|
delete map_program;
|
||||||
|
m_object.destroy();
|
||||||
|
m_vertex.destroy();
|
||||||
|
delete m_program;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::load_texture(int len, Vertex *ver, QImage *map_img) {
|
||||||
|
|
||||||
|
map_redraw = 0;
|
||||||
|
if (map_texture != NULL)
|
||||||
|
map_texture->destroy();
|
||||||
|
map_texture = new QOpenGLTexture(*map_img);
|
||||||
|
vertices_len = len;
|
||||||
|
memcpy(vertices, ver, len);
|
||||||
|
map_vertex.bind();
|
||||||
|
map_vertex.write(0, vertices, vertices_len);
|
||||||
|
emit(map_loaded());
|
||||||
|
|
||||||
|
map_redraw = 1;
|
||||||
|
this->update();
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapView::keyPressEvent(QKeyEvent *key) { emit(keyPress(key)); }
|
||||||
|
void MapView::mousePressEvent(QMouseEvent *event) { emit(mouse_press(event)); }
|
||||||
231
qt_pi/src/speller.cpp
Normal file
231
qt_pi/src/speller.cpp
Normal file
@ -0,0 +1,231 @@
|
|||||||
|
#include "speller/speller.h"
|
||||||
|
Speller::Speller(int ar, char **av) {
|
||||||
|
|
||||||
|
argc = ar;
|
||||||
|
argv = av;
|
||||||
|
spelling = 0;
|
||||||
|
|
||||||
|
window = new MapView();
|
||||||
|
|
||||||
|
connect(window, SIGNAL(keyPress(QKeyEvent *)), this,
|
||||||
|
SLOT(keyEvent(QKeyEvent *)));
|
||||||
|
connect(this, SIGNAL(load_texture(int, Vertex *, QImage *)), window,
|
||||||
|
SLOT(load_texture(int, Vertex *, QImage *)));
|
||||||
|
connect(window, SIGNAL(map_loaded()), this, SLOT(map_loaded()));
|
||||||
|
connect(window, SIGNAL(mouse_press(QMouseEvent *)), this,
|
||||||
|
SLOT(mouse_press(QMouseEvent *)));
|
||||||
|
}
|
||||||
|
|
||||||
|
Speller::~Speller() {
|
||||||
|
|
||||||
|
insight_stop_notif();
|
||||||
|
insight_destroy();
|
||||||
|
ros::shutdown();
|
||||||
|
delete window;
|
||||||
|
this->wait();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Speller::run() {
|
||||||
|
|
||||||
|
// init_ble();
|
||||||
|
init_ros();
|
||||||
|
load_map();
|
||||||
|
ros::Rate loop(10);
|
||||||
|
ros::Rate loop2(10);
|
||||||
|
while (ros::ok()) {
|
||||||
|
loop2.sleep();
|
||||||
|
while (spelling && ros::ok()) {
|
||||||
|
printf("Shape %d\n", map_shape);
|
||||||
|
get_ble_data();
|
||||||
|
// TODO-Send data for classification
|
||||||
|
// check classifier result();
|
||||||
|
classify(buf_len, buf);
|
||||||
|
printf("classifier %d\n", classifier_result);
|
||||||
|
reconfigure_map(classifier_result, map_shape);
|
||||||
|
load_map();
|
||||||
|
loop.sleep();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Speller::classify(int len, struct insight_data *buf) {
|
||||||
|
sleep(2);
|
||||||
|
if ((me.x() * me.y()) < 0) {
|
||||||
|
if (me.x() < 0)
|
||||||
|
classifier_result = TOP_LEFT;
|
||||||
|
else
|
||||||
|
classifier_result = BOTTOM_RIGHT;
|
||||||
|
} else {
|
||||||
|
if (me.x() < 0)
|
||||||
|
classifier_result = BOTTOM_LEFT;
|
||||||
|
else
|
||||||
|
classifier_result = TOP_RIGHT;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Speller::reconfigure_map(int result, int shape) {
|
||||||
|
int w = area_displayed->width();
|
||||||
|
int h = area_displayed->height();
|
||||||
|
int x = area_displayed->x();
|
||||||
|
int y = area_displayed->y();
|
||||||
|
|
||||||
|
switch (shape) {
|
||||||
|
case SQUARE:
|
||||||
|
switch (result) {
|
||||||
|
case TOP_LEFT:
|
||||||
|
area_displayed->setWidth(w / 1.8);
|
||||||
|
area_displayed->setY(y + h / 1.8);
|
||||||
|
break;
|
||||||
|
case TOP_RIGHT:
|
||||||
|
area_displayed->setX(x + w / 1.8);
|
||||||
|
area_displayed->setY(y + h / 1.8);
|
||||||
|
break;
|
||||||
|
case BOTTOM_LEFT:
|
||||||
|
area_displayed->setHeight(h / 1.8);
|
||||||
|
area_displayed->setWidth(w / 1.8);
|
||||||
|
break;
|
||||||
|
case BOTTOM_RIGHT:
|
||||||
|
area_displayed->setHeight(h / 1.8);
|
||||||
|
area_displayed->setX(x + w / 1.8);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case HORIZONTAL:
|
||||||
|
switch (result) {
|
||||||
|
case TOP_LEFT:
|
||||||
|
case BOTTOM_LEFT:
|
||||||
|
area_displayed->setWidth(w / 1.8);
|
||||||
|
break;
|
||||||
|
case TOP_RIGHT:
|
||||||
|
case BOTTOM_RIGHT:
|
||||||
|
area_displayed->setX(x + w / 1.8);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case VERTICAL:
|
||||||
|
switch (result) {
|
||||||
|
case TOP_LEFT:
|
||||||
|
case TOP_RIGHT:
|
||||||
|
area_displayed->setY(h + h / 1.8);
|
||||||
|
break;
|
||||||
|
case BOTTOM_LEFT:
|
||||||
|
case BOTTOM_RIGHT:
|
||||||
|
area_displayed->setHeight(h / 1.8);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
__inline__ void Speller::load_map() {
|
||||||
|
float max = std::max(area_displayed->width(), area_displayed->height());
|
||||||
|
|
||||||
|
float w = area_displayed->width() / max;
|
||||||
|
float h = area_displayed->height() / max;
|
||||||
|
float aspect_ratio = w / h;
|
||||||
|
|
||||||
|
if (aspect_ratio > 1.5)
|
||||||
|
map_shape = HORIZONTAL;
|
||||||
|
else if (aspect_ratio < 0.5)
|
||||||
|
map_shape = VERTICAL;
|
||||||
|
else
|
||||||
|
map_shape = SQUARE;
|
||||||
|
|
||||||
|
Vertex tmp_info[] = {
|
||||||
|
Vertex(QVector3D(-w, -h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
|
||||||
|
QVector2D(0.0f, 0.0f)),
|
||||||
|
Vertex(QVector3D(-w, h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
|
||||||
|
QVector2D(0.0f, 1.0f)),
|
||||||
|
Vertex(QVector3D(w, h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
|
||||||
|
QVector2D(1.0, 1.0f)),
|
||||||
|
Vertex(QVector3D(w, -h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
|
||||||
|
QVector2D(1.0f, 0.0f))};
|
||||||
|
|
||||||
|
QImage tmp = curr_map->copy(*area_displayed);
|
||||||
|
|
||||||
|
emit(load_texture(sizeof(tmp_info), tmp_info, &tmp));
|
||||||
|
|
||||||
|
map_loading_done = 0;
|
||||||
|
|
||||||
|
while (!map_loading_done)
|
||||||
|
sleep(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
__inline__ void Speller::get_ble_data() {
|
||||||
|
|
||||||
|
window->start_blinking();
|
||||||
|
// int ret = insight_write_to_buffer(buf_len, buf);
|
||||||
|
|
||||||
|
// while ((!ret) && (insight_buffer_status() == 1))
|
||||||
|
// sleep(0);
|
||||||
|
sleep(2);
|
||||||
|
window->stop_blinking();
|
||||||
|
}
|
||||||
|
|
||||||
|
__inline__ void Speller::init_ros() {
|
||||||
|
|
||||||
|
ros::init(argc, argv, "speller");
|
||||||
|
|
||||||
|
nh = new ros::NodeHandle();
|
||||||
|
pub_goal =
|
||||||
|
nh->advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
|
||||||
|
map_client = nh->serviceClient<nav_msgs::GetMap>("/static_map");
|
||||||
|
|
||||||
|
// sub_pose = nh->subscribe("qt_pi/pose", 1, &Speller::poseCB, this);
|
||||||
|
|
||||||
|
nav_msgs::GetMap tmp;
|
||||||
|
while (!map_client.call(tmp))
|
||||||
|
sleep(0);
|
||||||
|
map = tmp.response.map;
|
||||||
|
|
||||||
|
map_tex_buffer = (uint8_t *)malloc(map.info.height * map.info.width * 3);
|
||||||
|
|
||||||
|
for (int i = 0; i < map.info.height * map.info.width; i++) {
|
||||||
|
if (map.data[i] > 50) {
|
||||||
|
map_tex_buffer[3 * i + 0] = 0;
|
||||||
|
map_tex_buffer[3 * i + 1] = 0;
|
||||||
|
map_tex_buffer[3 * i + 2] = 0;
|
||||||
|
} else {
|
||||||
|
map_tex_buffer[3 * i + 0] = 255;
|
||||||
|
map_tex_buffer[3 * i + 1] = 255;
|
||||||
|
map_tex_buffer[3 * i + 2] = 255;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
curr_map = new QImage(map_tex_buffer, map.info.width, map.info.height,
|
||||||
|
3 * map.info.width, QImage::Format_RGB888);
|
||||||
|
area_displayed = new QRect(QPoint(0, 0), curr_map->size());
|
||||||
|
}
|
||||||
|
|
||||||
|
void Speller::poseCB(nav_msgs::Odometry::ConstPtr &msg) {
|
||||||
|
pose_current = msg->pose.pose;
|
||||||
|
}
|
||||||
|
|
||||||
|
__inline__ void Speller::init_ble() {
|
||||||
|
|
||||||
|
const char *a = "EF:BD:39:4E:08:49";
|
||||||
|
const char *b = "81072f41-9f3d-11e3-a9dc-0002a5d5c51b";
|
||||||
|
|
||||||
|
if (!insight_init(a, b))
|
||||||
|
buf = (struct insight_data *)malloc(sizeof(struct insight_data) * buf_len);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Speller::map_loaded() { map_loading_done = 1; }
|
||||||
|
|
||||||
|
void Speller::keyEvent(QKeyEvent *e) {
|
||||||
|
|
||||||
|
if (e->key() == Qt::Key_Space) {
|
||||||
|
|
||||||
|
if (!spelling)
|
||||||
|
insight_start_notif();
|
||||||
|
else
|
||||||
|
insight_stop_notif();
|
||||||
|
spelling = !spelling;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Speller::mouse_press(QMouseEvent *e) {
|
||||||
|
me = e->pos();
|
||||||
|
me.setX(me.x() - 500);
|
||||||
|
me.setY(500 - me.y());
|
||||||
|
printf("%d %d\n", me.x(), me.y());
|
||||||
|
}
|
||||||
33
qt_pi/tests/ble_test.cpp
Normal file
33
qt_pi/tests/ble_test.cpp
Normal file
@ -0,0 +1,33 @@
|
|||||||
|
#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) * 1000);
|
||||||
|
|
||||||
|
int len = 1000;
|
||||||
|
|
||||||
|
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++) {
|
||||||
|
for (int j = 0; j < 5; j++)
|
||||||
|
printf("%f ", buf[i].uV[j]);
|
||||||
|
printf("\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("Destroy called %d\n", insight_destroy());
|
||||||
|
printf("%d\n", insight_version());
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
43
qt_pi/tests/map_load_test.cpp
Normal file
43
qt_pi/tests/map_load_test.cpp
Normal file
@ -0,0 +1,43 @@
|
|||||||
|
#include <QApplication>
|
||||||
|
#include <QImage>
|
||||||
|
#include <QLabel>
|
||||||
|
#include <nav_msgs/GetMap.h>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
int main(int argc, char *argv[]) {
|
||||||
|
QApplication a(argc, argv);
|
||||||
|
ros::init(argc, argv, "Holla");
|
||||||
|
|
||||||
|
QImage *myImage;
|
||||||
|
|
||||||
|
auto nh = new ros::NodeHandle();
|
||||||
|
|
||||||
|
auto map_client = nh->serviceClient<nav_msgs::GetMap>("/static_map");
|
||||||
|
|
||||||
|
nav_msgs::GetMap tmp;
|
||||||
|
while (!map_client.call(tmp))
|
||||||
|
sleep(0);
|
||||||
|
auto map = tmp.response.map;
|
||||||
|
|
||||||
|
uint8_t *map_tex = (uint8_t *)malloc(map.info.height * map.info.width * 3);
|
||||||
|
|
||||||
|
for (int i = 0; i < map.info.height * map.info.width; i++) {
|
||||||
|
if (map.data[i] > 50) {
|
||||||
|
map_tex[3 * i + 0] = 0;
|
||||||
|
map_tex[3 * i + 1] = 0;
|
||||||
|
map_tex[3 * i + 2] = 0;
|
||||||
|
} else {
|
||||||
|
map_tex[3 * i + 0] = 255;
|
||||||
|
map_tex[3 * i + 1] = 255;
|
||||||
|
map_tex[3 * i + 2] = 255;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
QImage *tmp_ = (new QImage(map_tex, map.info.width, map.info.height,
|
||||||
|
3 * map.info.width, QImage::Format_RGB888));
|
||||||
|
QLabel myLabel;
|
||||||
|
myLabel.setPixmap(QPixmap::fromImage(tmp_->mirrored()));
|
||||||
|
|
||||||
|
myLabel.show();
|
||||||
|
|
||||||
|
return a.exec();
|
||||||
|
}
|
||||||
@ -1,35 +0,0 @@
|
|||||||
#ifndef COMMANDS_H
|
|
||||||
#define COMMANDS_H
|
|
||||||
|
|
||||||
#define INVALID 0xFF
|
|
||||||
#define INVALID_LENGTH 0xFE
|
|
||||||
|
|
||||||
#define GET_BAUDRATE 0x51
|
|
||||||
#define GET_ENCODERS 0x52
|
|
||||||
#define SET_SPEEDS 0x53
|
|
||||||
#define RESET_ENCODERS 0x54
|
|
||||||
#define SET_PID_L 0x55
|
|
||||||
#define SET_PID_R 0x56
|
|
||||||
#define GET_PID_P 0x57
|
|
||||||
#define GET_BATTERY_LVL 0x58
|
|
||||||
#define EN_IMU 0x59
|
|
||||||
#define EN_ODOM 0x60
|
|
||||||
#define ODOM_DATA 0x61
|
|
||||||
#define IMU_DATA 0x62
|
|
||||||
|
|
||||||
#define GET_BAUDRATE_LEN 0x01
|
|
||||||
#define GET_ENCODERS_LEN 0x01
|
|
||||||
#define SET_SPEEDS_LEN 0x03
|
|
||||||
#define RESET_ENCODERS_LEN 0x01
|
|
||||||
#define SET_PID_LEN 0x05
|
|
||||||
#define GET_PID_P_LEN 0x01
|
|
||||||
#define GET_BATTERY_LVL_LEN 0x01
|
|
||||||
#define EN_IMU_LEN 0x02
|
|
||||||
#define EN_ODOM_LEN 0x02
|
|
||||||
#define ODOM_DATA_LEN 0x08
|
|
||||||
#define IMU_DATA_LEN 0x08
|
|
||||||
|
|
||||||
#define LEFT 0x00
|
|
||||||
#define RIGHT 0x01
|
|
||||||
|
|
||||||
#endif
|
|
||||||
@ -1,27 +0,0 @@
|
|||||||
#ifndef PAYLOAD_SPECS_H
|
|
||||||
#define PAYLOAD_SPECS_H
|
|
||||||
|
|
||||||
#define MB1P 0
|
|
||||||
#define MB2P 1
|
|
||||||
#define LB1P 2
|
|
||||||
#define LB2P 3
|
|
||||||
#define CIDBP 4
|
|
||||||
#define COMBP 5
|
|
||||||
#define DATASTRBP 6
|
|
||||||
|
|
||||||
#define start_magic_len 2
|
|
||||||
#define end_magic_len 1
|
|
||||||
#define len_len 2
|
|
||||||
|
|
||||||
#define BID 0xFF
|
|
||||||
|
|
||||||
#define CIDB 0
|
|
||||||
#define COMB 1
|
|
||||||
#define DATASTRB 2
|
|
||||||
|
|
||||||
const int com_preamble_len = 2;
|
|
||||||
|
|
||||||
const int preamble_len = start_magic_len + len_len;
|
|
||||||
const int net_extra_len = preamble_len + end_magic_len;
|
|
||||||
|
|
||||||
#endif
|
|
||||||
Loading…
x
Reference in New Issue
Block a user