diff --git a/.gitignore b/.gitignore index 652a2fa..eb0f44a 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ -/scripts/qt_pi/*.pyc +*.pyc +.ccls-cache +compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt index e3f3bad..191ccf8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,21 +1,3 @@ -cmake_minimum_required(VERSION 2.8.3) -project(qt_pi) -find_package(catkin REQUIRED) - -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} -) +cmake_minimum_required(VERSION 3.5) +ADD_SUBDIRECTORY( qt_pi ) +ADD_SUBDIRECTORY( gazebo_sim ) diff --git a/gazebo_sim/CMakeLists.txt b/gazebo_sim/CMakeLists.txt new file mode 100644 index 0000000..94fdf7d --- /dev/null +++ b/gazebo_sim/CMakeLists.txt @@ -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) diff --git a/config/base_local_planner_params.yaml b/gazebo_sim/config/base_local_planner_params.yaml similarity index 100% rename from config/base_local_planner_params.yaml rename to gazebo_sim/config/base_local_planner_params.yaml diff --git a/gazebo_sim/config/costmap_common_params.yaml b/gazebo_sim/config/costmap_common_params.yaml new file mode 100644 index 0000000..d569971 --- /dev/null +++ b/gazebo_sim/config/costmap_common_params.yaml @@ -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} diff --git a/gazebo_sim/config/global_costmap_params.yaml b/gazebo_sim/config/global_costmap_params.yaml new file mode 100644 index 0000000..090b274 --- /dev/null +++ b/gazebo_sim/config/global_costmap_params.yaml @@ -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 diff --git a/gazebo_sim/config/local_costmap_params.yaml b/gazebo_sim/config/local_costmap_params.yaml new file mode 100644 index 0000000..a6f3c45 --- /dev/null +++ b/gazebo_sim/config/local_costmap_params.yaml @@ -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 diff --git a/gazebo_sim/launch/depth_to_laser.launch b/gazebo_sim/launch/depth_to_laser.launch new file mode 100644 index 0000000..ac1b042 --- /dev/null +++ b/gazebo_sim/launch/depth_to_laser.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/gazebo_sim/launch/gazebo_sim.launch b/gazebo_sim/launch/gazebo_sim.launch new file mode 100644 index 0000000..64b5ae2 --- /dev/null +++ b/gazebo_sim/launch/gazebo_sim.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/gazebo_sim/launch/gmapping.launch b/gazebo_sim/launch/gmapping.launch new file mode 100644 index 0000000..03482e0 --- /dev/null +++ b/gazebo_sim/launch/gmapping.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_sim/launch/move_base_amcl.launch b/gazebo_sim/launch/move_base_amcl.launch new file mode 100644 index 0000000..573d3ee --- /dev/null +++ b/gazebo_sim/launch/move_base_amcl.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_sim/launch/move_base_gmapping.launch b/gazebo_sim/launch/move_base_gmapping.launch new file mode 100644 index 0000000..f138dcf --- /dev/null +++ b/gazebo_sim/launch/move_base_gmapping.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_sim/launch/static_tf.launch b/gazebo_sim/launch/static_tf.launch new file mode 100644 index 0000000..1735590 --- /dev/null +++ b/gazebo_sim/launch/static_tf.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/gazebo_sim/launch/teleop.launch b/gazebo_sim/launch/teleop.launch new file mode 100644 index 0000000..58d8b2f --- /dev/null +++ b/gazebo_sim/launch/teleop.launch @@ -0,0 +1,4 @@ + + + + diff --git a/gazebo_sim/map/frames.gv b/gazebo_sim/map/frames.gv new file mode 100644 index 0000000..8108a1a --- /dev/null +++ b/gazebo_sim/map/frames.gv @@ -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"; +} \ No newline at end of file diff --git a/gazebo_sim/map/frames.pdf b/gazebo_sim/map/frames.pdf new file mode 100644 index 0000000..dc087d8 Binary files /dev/null and b/gazebo_sim/map/frames.pdf differ diff --git a/gazebo_sim/map/playground.pgm b/gazebo_sim/map/playground.pgm new file mode 100644 index 0000000..f4af84d Binary files /dev/null and b/gazebo_sim/map/playground.pgm differ diff --git a/gazebo_sim/map/playground.yaml b/gazebo_sim/map/playground.yaml new file mode 100644 index 0000000..bd22da1 --- /dev/null +++ b/gazebo_sim/map/playground.yaml @@ -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 diff --git a/gazebo_sim/package.xml b/gazebo_sim/package.xml new file mode 100644 index 0000000..7486499 --- /dev/null +++ b/gazebo_sim/package.xml @@ -0,0 +1,17 @@ + + qt_pi_gazebo_sim + 0.0.0 + The qt_pi_gazebo_sim package + Rishabh Kundu + MIT + catkin + gmapping + amcl + move_base + depthimage_to_laserscan + map_server + static_transform_publisher + teleop_twist_keyboard + + + diff --git a/gazebo_sim/worlds/playground.world b/gazebo_sim/worlds/playground.world new file mode 100644 index 0000000..5a5f919 --- /dev/null +++ b/gazebo_sim/worlds/playground.world @@ -0,0 +1,2046 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 0.193814 -0.057177 0 0 -0 0 + + + + + 26.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 26.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0.007886 -3.91706 0 0 -0 0 + 0 + 0 + 0 + + + + + + 8 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 8 0.15 2.5 + + + + + 1 1 1 1 + + + 13.3079 0.007941 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 26.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 26.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0.007886 3.93294 0 0 -0 3.14159 + 0 + 0 + 0 + + + -9.18476 0.019386 0 0 0 -1.5708 + + -3.12221 0 1.25 0 -0 0 + + + 1.75558 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.75558 0.15 2.5 + + + -3.12221 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.438191 0 1.25 0 -0 0 + + + 3.56521 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.56521 0.15 2.5 + + + 0.438191 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 3.5604 0 1.25 0 -0 0 + + + 0.879203 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.879203 0.15 2.5 + + + 3.5604 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -1.79442 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -1.79442 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.6708 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 2.6708 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 4.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.25 0.15 2.5 + + + + + 1 1 1 1 + + + -11.2579 -1.35252 0 0 -0 3.14159 + 0 + 0 + 0 + + + -6.89009 -0.019386 0 0 0 -1.5708 + + -2.05546 0 1.25 0 -0 0 + + + 3.88908 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.88908 0.15 2.5 + + + -2.05546 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.39454 0 1.25 0 -0 0 + + + 3.21092 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.21092 0.15 2.5 + + + 2.39454 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.339077 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.339077 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + -2.84009 1.17017 0 0 -0 0 + + -2.27097 0 1.25 0 -0 0 + + + 3.70805 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.70805 0.15 2.5 + + + -2.27097 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.30403 0 1.25 0 -0 0 + + + 3.64195 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.64195 0.15 2.5 + + + 2.30403 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.033055 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.033055 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 1.21568 0.006434 0 0 -0 1.5708 + + -2.30688 0 1.25 0 -0 0 + + + 3.38624 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.38624 0.15 2.5 + + + -2.30688 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.14312 0 1.25 0 -0 0 + + + 3.71376 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.71376 0.15 2.5 + + + 2.14312 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -0.163762 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -0.163762 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5 0.15 2.5 + + + + + 1 1 1 1 + + + 3.62057 -1.58539 0 0 -0 0 + 0 + 0 + 0 + + + 6.04557 -2.76039 0 0 0 -1.5708 + + -0.84415 0 1.25 0 -0 0 + + + 0.811701 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.811701 0.15 2.5 + + + -0.84415 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.85585 0 1.25 0 -0 0 + + + 0.788299 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.788299 0.15 2.5 + + + 0.85585 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.011701 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.011701 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 3.40806 1.10972 0 0 -0 1.5708 + + -2.20336 0 1.25 0 -0 0 + + + 1.09329 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.09329 0.15 2.5 + + + -2.20336 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.181127 0 1.25 0 -0 0 + + + 1.87567 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.87567 0.15 2.5 + + + 0.181127 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.38448 0 1.25 0 -0 0 + + + 0.731036 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.731036 0.15 2.5 + + + 2.38448 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -1.20671 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -1.20671 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.56896 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 1.56896 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 4.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.5 0.15 2.5 + + + + + 1 1 1 1 + + + 5.60318 1.67301 0 0 -0 0 + 0 + 0 + 0 + + + 7.77818 2.72301 0 0 -0 1.5708 + + -0.729206 0 1.25 0 -0 0 + + + 0.791587 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.791587 0.15 2.5 + + + -0.729206 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.845794 0 1.25 0 -0 0 + + + 0.558413 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.558413 0.15 2.5 + + + 0.845794 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.116587 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.116587 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 10.5278 0.085507 0 0 0 -0.523599 + + -0.945166 0 1.25 0 -0 0 + + + 4.60967 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 4.60967 0.15 2.5 + + + -0.945166 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.75483 0 1.25 0 -0 0 + + + 0.990333 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.990333 0.15 2.5 + + + 2.75483 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.80967 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 1.80967 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 10.3472 -1.82517 0 0 0 -1.5708 + + -0.985004 0 1.25 0 -0 0 + + + 2.27999 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 2.27999 0.15 2.5 + + + -0.985004 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.59 0 1.25 0 -0 0 + + + 1.07001 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.07001 0.15 2.5 + + + 1.59 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.604992 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.604992 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 8 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 8 0.15 2.5 + + + + + 1 1 1 1 + + + -13.2921 0.007941 0 0 0 -1.5708 + 0 + 0 + 0 + + 1 + + + + 0 0 0.1 0 -0 0 + + 20 + 0.1 0 -0.05 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + 0.4 0.2 0.1 + + + 10 + + + + + + + + + + + + + + + + + 0.4 0.2 0.1 + + + + + 0.15 0 -0.05 0 -0 0 + + + 0.05 + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + 0 + 0 + 1 + 1 + + + + + + + 10 + + + 0.15 0 -0.05 0 -0 0 + + + 0.05 + + + + 0 + 0 + 0 + + + -0.1 0.2 0.1 0 -0 0 + + 5 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + + 10 + 10 + + + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + + 0.1 + + + 10 + + + + + 0.1 + + + + 0 + 0 + 0 + + + -0.1 -0.2 0.1 0 -0 0 + + 5 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + + 10 + 10 + + + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + + 0.1 + + + 10 + + + + + 0.1 + + + + 0 + 0 + 0 + + + 0 0 0 0 -0 0 + left_wheel + chassis + + 0 1 0 + 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + right_wheel + chassis + + 0 1 0 + 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + + 0.1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + 0.073 0.276 0.072 + + + 10 + + + + + + + + + + + + + + + + + model://kinect/meshes/kinect.dae + + + + + 15 + + 1.2215 + + 640 + 480 + R8G8B8 + + + 0.05 + 100 + + + + qt_pi + 0.2 + 1 + 15 + camera_ir + /qt_pi/camera_ir/color/image_raw + /qt_pi/camera_ir/color/camera_info + /qt_pi/camera_ir/depth/image_raw + /camera_ir/depth/camera_info + /qt_pi/camera_ir/depth/points + camera_link + 100 + 0.05 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + 0.2 0 0.2 0 -0 0 + 0 + 0 + 0 + + + kinect::link + chassis + + + 1 + 30 + left_wheel_hinge + qt_pi + right_wheel_hinge + 0.4 + 0.2 + 0 + 5 + cmd_vel + odom + base_link + odom + 1 + 1 + 1 + 0 + 0 + + 2.20517 -4.60502 0 0 -0 0 + + + 47 5000000 + 47 576261785 + 1554061633 217805104 + 47005 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0.007886 -3.91706 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 13.3079 0.007941 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.007886 3.93294 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -9.18476 0.019386 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -11.2579 -1.35252 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89009 -0.019386 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.84009 1.17017 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.21568 0.006434 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.62057 -1.58539 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.04557 -2.76039 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.40806 1.10972 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.60318 1.67301 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.77818 2.72301 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.5278 0.085507 0 0 0 -0.523599 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3472 -1.82517 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2921 0.007941 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 0 0 + 1 1 1 + + 0 0 0.1 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + + + 0.2 0 0.2 0 0 0 + 0 -0 0 0 0 0 + -0 0 0 0 0 0 + -0 0 0 0 0 0 + + + -0.1 0.2 0.1 0 0 0 + 0 -0 0 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 + + + -0.1 -0.2 0.1 0 0 0 + -0 0 0 0 0 0 + 0 0 0 0 0 0 + -0 0 0 0 0 0 + + + + 0 0 10 0 0 0 + + + + + 6.80207 -4.04261 25.2995 -4.6e-05 1.5698 1.10815 + orbit + perspective + + + + \ No newline at end of file diff --git a/gazebo_sim/worlds/qt_pi_laser.world b/gazebo_sim/worlds/qt_pi_laser.world new file mode 100644 index 0000000..be5f509 --- /dev/null +++ b/gazebo_sim/worlds/qt_pi_laser.world @@ -0,0 +1,2061 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + -1.25415 -0.788564 0 0 -0 0 + + + + + 26.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 26.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0.007886 -3.91706 0 0 -0 0 + 0 + 0 + 0 + + + + + + 8 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 8 0.15 2.5 + + + + + 1 1 1 1 + + + 13.3079 0.007941 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 26.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 26.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0.007886 3.93294 0 0 -0 3.14159 + 0 + 0 + 0 + + + -9.18476 0.019386 0 0 0 -1.5708 + + -3.12221 0 1.25 0 -0 0 + + + 1.75558 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.75558 0.15 2.5 + + + -3.12221 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.438191 0 1.25 0 -0 0 + + + 3.56521 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.56521 0.15 2.5 + + + 0.438191 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 3.5604 0 1.25 0 -0 0 + + + 0.879203 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.879203 0.15 2.5 + + + 3.5604 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -1.79442 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -1.79442 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.6708 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 2.6708 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 4.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.25 0.15 2.5 + + + + + 1 1 1 1 + + + -11.2579 -1.35252 0 0 -0 3.14159 + 0 + 0 + 0 + + + -6.89009 -0.019386 0 0 0 -1.5708 + + -2.05546 0 1.25 0 -0 0 + + + 3.88908 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.88908 0.15 2.5 + + + -2.05546 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.39454 0 1.25 0 -0 0 + + + 3.21092 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.21092 0.15 2.5 + + + 2.39454 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.339077 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.339077 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + -2.84009 1.17017 0 0 -0 0 + + -2.27097 0 1.25 0 -0 0 + + + 3.70805 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.70805 0.15 2.5 + + + -2.27097 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.30403 0 1.25 0 -0 0 + + + 3.64195 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.64195 0.15 2.5 + + + 2.30403 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.033055 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.033055 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 1.21568 0.006434 0 0 -0 1.5708 + + -2.30688 0 1.25 0 -0 0 + + + 3.38624 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.38624 0.15 2.5 + + + -2.30688 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.14312 0 1.25 0 -0 0 + + + 3.71376 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 3.71376 0.15 2.5 + + + 2.14312 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -0.163762 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -0.163762 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5 0.15 2.5 + + + + + 1 1 1 1 + + + 3.62057 -1.58539 0 0 -0 0 + 0 + 0 + 0 + + + 6.04557 -2.76039 0 0 0 -1.5708 + + -0.84415 0 1.25 0 -0 0 + + + 0.811701 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.811701 0.15 2.5 + + + -0.84415 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.85585 0 1.25 0 -0 0 + + + 0.788299 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.788299 0.15 2.5 + + + 0.85585 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.011701 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.011701 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 3.40806 1.10972 0 0 -0 1.5708 + + -2.20336 0 1.25 0 -0 0 + + + 1.09329 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.09329 0.15 2.5 + + + -2.20336 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.181127 0 1.25 0 -0 0 + + + 1.87567 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.87567 0.15 2.5 + + + 0.181127 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.38448 0 1.25 0 -0 0 + + + 0.731036 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.731036 0.15 2.5 + + + 2.38448 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -1.20671 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + -1.20671 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.56896 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 1.56896 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 4.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.5 0.15 2.5 + + + + + 1 1 1 1 + + + 5.60318 1.67301 0 0 -0 0 + 0 + 0 + 0 + + + 7.77818 2.72301 0 0 -0 1.5708 + + -0.729206 0 1.25 0 -0 0 + + + 0.791587 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.791587 0.15 2.5 + + + -0.729206 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.845794 0 1.25 0 -0 0 + + + 0.558413 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.558413 0.15 2.5 + + + 0.845794 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.116587 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.116587 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 10.5278 0.085507 0 0 0 -0.523599 + + -0.945166 0 1.25 0 -0 0 + + + 4.60967 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 4.60967 0.15 2.5 + + + -0.945166 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 2.75483 0 1.25 0 -0 0 + + + 0.990333 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 0.990333 0.15 2.5 + + + 2.75483 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.80967 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 1.80967 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + 10.3472 -1.82517 0 0 0 -1.5708 + + -0.985004 0 1.25 0 -0 0 + + + 2.27999 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 2.27999 0.15 2.5 + + + -0.985004 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.59 0 1.25 0 -0 0 + + + 1.07001 0.15 2.5 + + + + + 1 1 1 1 + + + + + + 1.07001 0.15 2.5 + + + 1.59 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.604992 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + + + + 0.9 0.15 0.5 + + + 0.604992 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 8 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 8 0.15 2.5 + + + + + 1 1 1 1 + + + -13.2921 0.007941 0 0 0 -1.5708 + 0 + 0 + 0 + + 1 + + + + 0 0 0.1 0 -0 0 + + 20 + 0.1 0 -0.05 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + 0.4 0.2 0.1 + + + 10 + + + + + + + + + + + + + + + + + 0.4 0.2 0.1 + + + + + 0.15 0 -0.05 0 -0 0 + + + 0.05 + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + 0 + 0 + 1 + 1 + + + + + + + 10 + + + 0.15 0 -0.05 0 -0 0 + + + 0.05 + + + + 0 + 0 + 0 + + + -0.1 0.2 0.1 0 -0 0 + + 5 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + + 10 + 10 + + + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + + 0.1 + + + 10 + + + + + 0.1 + + + + 0 + 0 + 0 + + + -0.1 -0.2 0.1 0 -0 0 + + 5 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + + 10 + 10 + + + + + + + 0 + 0.01 + + + + 0 + 0.001 + + + + + + 0.1 + + + 10 + + + + + 0.1 + + + + 0 + 0 + 0 + + + 0 0 0 0 -0 0 + left_wheel + chassis + + 0 1 0 + 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + right_wheel + chassis + + 0 1 0 + 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + + 0.1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + model://hokuyo/meshes/hokuyo.dae + + + + + 0 0 -0.0145 0 -0 0 + + + 0.05 0.05 0.041 + + + 10 + + + + + + + + + + + + + + + 0 0 0.0205 0 -0 0 + + + 0.021 + 0.029 + + + 10 + + + + + + + + + + + + + + + 0.01 0 0.0175 0 -0 0 + + + + 640 + 1 + -2.26889 + 2.2689 + + + 1 + 0 + 0 + + + + 0.08 + 10 + 0.01 + + + + scan + laser + 1 + qt_pi + 15 + + 15 + 1 + + 0.2 0 0.2 0 -0 0 + 0 + 0 + 0 + + + hokuyo::link + chassis + + + 1 + 30 + left_wheel_hinge + qt_pi + right_wheel_hinge + 0.4 + 0.2 + 0 + 5 + cmd_vel + odom + base_link + odom + 1 + 1 + 1 + 0 + 0 + + -0.306574 -6.64995 0 0 -0 0 + + + 65 341000000 + 65 969333035 + 1554402610 693687615 + 65341 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0.007886 -3.91706 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 13.3079 0.007941 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.007886 3.93294 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -9.18476 0.019386 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -11.2579 -1.35252 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -6.89009 -0.019386 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.84009 1.17017 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.21568 0.006434 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.62057 -1.58539 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.04557 -2.76039 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.40806 1.10972 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.60318 1.67301 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.77818 2.72301 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.5278 0.085507 0 0 0 -0.523599 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 10.3472 -1.82517 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -13.2921 0.007941 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0 0 0 0 0 0 + 1 1 1 + + 0 0 0.1 0 0 0 + 0 -0 -0 0 0 0 + 0 -0 -0 0 -0 0 + 0 -0 -0 0 -0 0 + + + 0.2 0 0.2 0 0 0 + 0 -0 -0 0 0 0 + 0 -0 0 0 -0 0 + 0 -0 0 0 -0 0 + + + -0.1 0.2 0.1 0 0 0 + 0 -0 0 0 0 0 + 0 -0 -0 0 -0 0 + 0 -0 -0 0 -0 0 + + + -0.1 -0.2 0.1 0 0 0 + -0 -0 -0 0 -0 0 + -0 -0 -0 0 -0 0 + -0 -0 -0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + -4.72192 -0.583216 33.7134 -0 1.5418 0.060154 + orbit + perspective + + + + \ No newline at end of file diff --git a/include/qt_pi/serial_driver.h b/include/qt_pi/serial_driver.h deleted file mode 100644 index e30c957..0000000 --- a/include/qt_pi/serial_driver.h +++ /dev/null @@ -1,9 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include "commands.h" -#include "payload_specs.h" diff --git a/package.xml b/package.xml deleted file mode 100644 index 407d1a7..0000000 --- a/package.xml +++ /dev/null @@ -1,17 +0,0 @@ - - qt_pi - 0.0.0 - The qt_pi package - Rishabh Kundu - MIT - catkin - - rospy - std_msgs - geometry_msgs - nav_msgs - tf - python-serial - - - diff --git a/qt_pi/CMakeLists.txt b/qt_pi/CMakeLists.txt new file mode 100644 index 0000000..d858d67 --- /dev/null +++ b/qt_pi/CMakeLists.txt @@ -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) diff --git a/README.md b/qt_pi/README.md similarity index 100% rename from README.md rename to qt_pi/README.md diff --git a/qt_pi/config/70-emotiv.rules b/qt_pi/config/70-emotiv.rules new file mode 100644 index 0000000..a6c4e05 --- /dev/null +++ b/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" diff --git a/qt_pi/config/base_local_planner_params.yaml b/qt_pi/config/base_local_planner_params.yaml new file mode 100644 index 0000000..3eadf53 --- /dev/null +++ b/qt_pi/config/base_local_planner_params.yaml @@ -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 diff --git a/config/costmap_common_params.yaml b/qt_pi/config/costmap_common_params.yaml similarity index 100% rename from config/costmap_common_params.yaml rename to qt_pi/config/costmap_common_params.yaml diff --git a/config/global_costmap_params.yaml b/qt_pi/config/global_costmap_params.yaml similarity index 100% rename from config/global_costmap_params.yaml rename to qt_pi/config/global_costmap_params.yaml diff --git a/config/local_costmap_params.yaml b/qt_pi/config/local_costmap_params.yaml similarity index 100% rename from config/local_costmap_params.yaml rename to qt_pi/config/local_costmap_params.yaml diff --git a/config/qt_pi_arduino_params.yaml b/qt_pi/config/qt_pi_arduino_params.yaml similarity index 100% rename from config/qt_pi_arduino_params.yaml rename to qt_pi/config/qt_pi_arduino_params.yaml diff --git a/qt_pi/doc/Connecting on debian.md b/qt_pi/doc/Connecting on debian.md new file mode 100644 index 0000000..187a3f2 --- /dev/null +++ b/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: + + `` to `` + + `` to `` + +### 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]` diff --git a/qt_pi/include/ble_connect/ble_connect.h b/qt_pi/include/ble_connect/ble_connect.h new file mode 100644 index 0000000..d200262 --- /dev/null +++ b/qt_pi/include/ble_connect/ble_connect.h @@ -0,0 +1,33 @@ +#ifndef BLE_CONNECT +#define BLE_CONNECT +#include "gattlib.h" +#include +#include +#include +#include +#include +#include +#include + +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 diff --git a/qt_pi/include/mapview/mapview.h b/qt_pi/include/mapview/mapview.h new file mode 100644 index 0000000..c5ccaec --- /dev/null +++ b/qt_pi/include/mapview/mapview.h @@ -0,0 +1,71 @@ +#ifndef MAPVIEW +#define MAPVIEW + +#include "vertex.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 diff --git a/qt_pi/include/mapview/vertex.h b/qt_pi/include/mapview/vertex.h new file mode 100644 index 0000000..7b5f2dd --- /dev/null +++ b/qt_pi/include/mapview/vertex.h @@ -0,0 +1,88 @@ +#ifndef VERTEX_H +#define VERTEX_H + +#include +#include +#include + +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 diff --git a/qt_pi/include/speller/speller.h b/qt_pi/include/speller/speller.h new file mode 100644 index 0000000..bad4d9a --- /dev/null +++ b/qt_pi/include/speller/speller.h @@ -0,0 +1,72 @@ +#ifndef SPELLER +#define SPELLER + +#include "ble_connect/ble_connect.h" +#include "mapview/mapview.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Speller : public QThread { + Q_OBJECT +public: + explicit Speller(int argc, char **argv); + ~Speller(); + const std::map 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 diff --git a/launch/arduino.launch b/qt_pi/launch/arduino.launch similarity index 100% rename from launch/arduino.launch rename to qt_pi/launch/arduino.launch diff --git a/launch/depth_to_laser.launch b/qt_pi/launch/depth_to_laser.launch similarity index 100% rename from launch/depth_to_laser.launch rename to qt_pi/launch/depth_to_laser.launch diff --git a/launch/gmapping.launch b/qt_pi/launch/gmapping.launch similarity index 100% rename from launch/gmapping.launch rename to qt_pi/launch/gmapping.launch diff --git a/launch/move_base.launch b/qt_pi/launch/move_base.launch similarity index 100% rename from launch/move_base.launch rename to qt_pi/launch/move_base.launch diff --git a/launch/qt_pi.launch b/qt_pi/launch/qt_pi.launch similarity index 100% rename from launch/qt_pi.launch rename to qt_pi/launch/qt_pi.launch diff --git a/qt_pi/msg/EEG_data.msg b/qt_pi/msg/EEG_data.msg new file mode 100644 index 0000000..60767b2 --- /dev/null +++ b/qt_pi/msg/EEG_data.msg @@ -0,0 +1,4 @@ +int channel[5] +float32 data[5] +int cq[5] +int batt_lvl diff --git a/nodes/arduino_node.py b/qt_pi/nodes/arduino_node.py similarity index 100% rename from nodes/arduino_node.py rename to qt_pi/nodes/arduino_node.py diff --git a/qt_pi/package.xml b/qt_pi/package.xml new file mode 100644 index 0000000..be01063 --- /dev/null +++ b/qt_pi/package.xml @@ -0,0 +1,18 @@ + + qt_pi + 0.0.0 + The qt_pi package + Rishabh Kundu + MIT + catkin + qt_build + roscpp + roslib + + rospy + geometry_msgs + nav_msgs + tf + python-serial + + \ No newline at end of file diff --git a/qt_pi/resources/resources.qrc b/qt_pi/resources/resources.qrc new file mode 100644 index 0000000..a8bbdea --- /dev/null +++ b/qt_pi/resources/resources.qrc @@ -0,0 +1,8 @@ + + + shaders/blink.frag + shaders/blink.vert + shaders/map.vert + shaders/map.frag + + diff --git a/qt_pi/resources/shaders/blink.frag b/qt_pi/resources/shaders/blink.frag new file mode 100644 index 0000000..ed9c30e --- /dev/null +++ b/qt_pi/resources/shaders/blink.frag @@ -0,0 +1,8 @@ +#version 330 +in vec4 vColor; +out vec4 fColor; + +void main() +{ + fColor = vColor; +} diff --git a/qt_pi/resources/shaders/blink.vert b/qt_pi/resources/shaders/blink.vert new file mode 100644 index 0000000..e4f8480 --- /dev/null +++ b/qt_pi/resources/shaders/blink.vert @@ -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; +} diff --git a/qt_pi/resources/shaders/map.frag b/qt_pi/resources/shaders/map.frag new file mode 100644 index 0000000..7d20a87 --- /dev/null +++ b/qt_pi/resources/shaders/map.frag @@ -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); +} diff --git a/qt_pi/resources/shaders/map.vert b/qt_pi/resources/shaders/map.vert new file mode 100644 index 0000000..49944eb --- /dev/null +++ b/qt_pi/resources/shaders/map.vert @@ -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; +} diff --git a/scripts/qt_pi/__init__.py b/qt_pi/scripts/qt_pi/__init__.py similarity index 100% rename from scripts/qt_pi/__init__.py rename to qt_pi/scripts/qt_pi/__init__.py diff --git a/scripts/qt_pi/arduino_driver.py b/qt_pi/scripts/qt_pi/arduino_driver.py similarity index 100% rename from scripts/qt_pi/arduino_driver.py rename to qt_pi/scripts/qt_pi/arduino_driver.py diff --git a/scripts/qt_pi/arduino_driver.pyc b/qt_pi/scripts/qt_pi/arduino_driver.pyc similarity index 100% rename from scripts/qt_pi/arduino_driver.pyc rename to qt_pi/scripts/qt_pi/arduino_driver.pyc diff --git a/scripts/qt_pi/base_controller.py b/qt_pi/scripts/qt_pi/base_controller.py similarity index 100% rename from scripts/qt_pi/base_controller.py rename to qt_pi/scripts/qt_pi/base_controller.py diff --git a/scripts/qt_pi/base_controller.pyc b/qt_pi/scripts/qt_pi/base_controller.pyc similarity index 100% rename from scripts/qt_pi/base_controller.pyc rename to qt_pi/scripts/qt_pi/base_controller.pyc diff --git a/setup.py b/qt_pi/setup.py similarity index 100% rename from setup.py rename to qt_pi/setup.py diff --git a/qt_pi/src/ble_connect.c b/qt_pi/src/ble_connect.c new file mode 100644 index 0000000..a2eccf0 --- /dev/null +++ b/qt_pi/src/ble_connect.c @@ -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; } diff --git a/src/libraries/Arduino/Arduino.ino b/qt_pi/src/libraries/Arduino/Arduino.ino similarity index 100% rename from src/libraries/Arduino/Arduino.ino rename to qt_pi/src/libraries/Arduino/Arduino.ino diff --git a/src/libraries/Arduino/batt.cpp b/qt_pi/src/libraries/Arduino/batt.cpp similarity index 100% rename from src/libraries/Arduino/batt.cpp rename to qt_pi/src/libraries/Arduino/batt.cpp diff --git a/src/libraries/Arduino/batt.h b/qt_pi/src/libraries/Arduino/batt.h similarity index 100% rename from src/libraries/Arduino/batt.h rename to qt_pi/src/libraries/Arduino/batt.h diff --git a/src/libraries/Arduino/comm.cpp b/qt_pi/src/libraries/Arduino/comm.cpp similarity index 100% rename from src/libraries/Arduino/comm.cpp rename to qt_pi/src/libraries/Arduino/comm.cpp diff --git a/src/libraries/Arduino/comm.h b/qt_pi/src/libraries/Arduino/comm.h similarity index 100% rename from src/libraries/Arduino/comm.h rename to qt_pi/src/libraries/Arduino/comm.h diff --git a/include/qt_pi/commands.h b/qt_pi/src/libraries/Arduino/commands.h similarity index 100% rename from include/qt_pi/commands.h rename to qt_pi/src/libraries/Arduino/commands.h diff --git a/src/libraries/Arduino/diff_controller.cpp b/qt_pi/src/libraries/Arduino/diff_controller.cpp similarity index 100% rename from src/libraries/Arduino/diff_controller.cpp rename to qt_pi/src/libraries/Arduino/diff_controller.cpp diff --git a/src/libraries/Arduino/diff_controller.h b/qt_pi/src/libraries/Arduino/diff_controller.h similarity index 100% rename from src/libraries/Arduino/diff_controller.h rename to qt_pi/src/libraries/Arduino/diff_controller.h diff --git a/src/libraries/Arduino/encoder_driver.cpp b/qt_pi/src/libraries/Arduino/encoder_driver.cpp similarity index 100% rename from src/libraries/Arduino/encoder_driver.cpp rename to qt_pi/src/libraries/Arduino/encoder_driver.cpp diff --git a/src/libraries/Arduino/encoder_driver.h b/qt_pi/src/libraries/Arduino/encoder_driver.h similarity index 100% rename from src/libraries/Arduino/encoder_driver.h rename to qt_pi/src/libraries/Arduino/encoder_driver.h diff --git a/src/libraries/Arduino/imu.cpp b/qt_pi/src/libraries/Arduino/imu.cpp similarity index 100% rename from src/libraries/Arduino/imu.cpp rename to qt_pi/src/libraries/Arduino/imu.cpp diff --git a/src/libraries/Arduino/imu.h b/qt_pi/src/libraries/Arduino/imu.h similarity index 100% rename from src/libraries/Arduino/imu.h rename to qt_pi/src/libraries/Arduino/imu.h diff --git a/src/libraries/Arduino/motor_driver.cpp b/qt_pi/src/libraries/Arduino/motor_driver.cpp similarity index 100% rename from src/libraries/Arduino/motor_driver.cpp rename to qt_pi/src/libraries/Arduino/motor_driver.cpp diff --git a/src/libraries/Arduino/motor_driver.h b/qt_pi/src/libraries/Arduino/motor_driver.h similarity index 100% rename from src/libraries/Arduino/motor_driver.h rename to qt_pi/src/libraries/Arduino/motor_driver.h diff --git a/include/qt_pi/payload_specs.h b/qt_pi/src/libraries/Arduino/payload_specs.h similarity index 100% rename from include/qt_pi/payload_specs.h rename to qt_pi/src/libraries/Arduino/payload_specs.h diff --git a/src/libraries/Arduino/qt_pi.h b/qt_pi/src/libraries/Arduino/qt_pi.h similarity index 100% rename from src/libraries/Arduino/qt_pi.h rename to qt_pi/src/libraries/Arduino/qt_pi.h diff --git a/qt_pi/src/main.cpp b/qt_pi/src/main.cpp new file mode 100644 index 0000000..89126f2 --- /dev/null +++ b/qt_pi/src/main.cpp @@ -0,0 +1,22 @@ +#include "speller/speller.h" +#include +#include + +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(); +} diff --git a/qt_pi/src/mapview.cpp b/qt_pi/src/mapview.cpp new file mode 100644 index 0000000..ce49d7d --- /dev/null +++ b/qt_pi/src/mapview.cpp @@ -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::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(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)); } diff --git a/src/serial_driver.cpp b/qt_pi/src/serial_driver.cpp similarity index 100% rename from src/serial_driver.cpp rename to qt_pi/src/serial_driver.cpp diff --git a/qt_pi/src/speller.cpp b/qt_pi/src/speller.cpp new file mode 100644 index 0000000..cf1aa35 --- /dev/null +++ b/qt_pi/src/speller.cpp @@ -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("/move_base_simple/goal", 1); + map_client = nh->serviceClient("/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()); +} diff --git a/qt_pi/tests/ble_test.cpp b/qt_pi/tests/ble_test.cpp new file mode 100644 index 0000000..c5295c4 --- /dev/null +++ b/qt_pi/tests/ble_test.cpp @@ -0,0 +1,33 @@ +#include "ble_connect/ble_connect.h" +#include +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; +} diff --git a/qt_pi/tests/map_load_test.cpp b/qt_pi/tests/map_load_test.cpp new file mode 100644 index 0000000..af47e1d --- /dev/null +++ b/qt_pi/tests/map_load_test.cpp @@ -0,0 +1,43 @@ +#include +#include +#include +#include +#include + +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("/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(); +} diff --git a/src/libraries/Arduino/commands.h b/src/libraries/Arduino/commands.h deleted file mode 100644 index 496f7da..0000000 --- a/src/libraries/Arduino/commands.h +++ /dev/null @@ -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 diff --git a/src/libraries/Arduino/payload_specs.h b/src/libraries/Arduino/payload_specs.h deleted file mode 100644 index bb98ac1..0000000 --- a/src/libraries/Arduino/payload_specs.h +++ /dev/null @@ -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