Temp Commit

.gitignore

@@ -1,3 +1,4 @@
 *.pyc
 .ccls-cache
 compile_commands.json
+tags

CMakeLists.txt

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

qt_pi/msg/EEG_data.msg

@@ -1,4 +0,0 @@
-int channel[5]
-float32 data[5]
-int cq[5]
-int batt_lvl

qt_pi/nodes/arduino_node.py

@@ -1,87 +0,0 @@
-#!/usr/bin/env python
-import rospy
-from qt_pi.arduino_driver import Arduino
-from qt_pi.base_controller import BaseController
-from geometry_msgs.msg import Twist
-import os, time
-import thread
-from serial.serialutil import SerialException
-
-class ArduinoROS():
-    def __init__(self):
-        rospy.init_node('arduino', log_level=rospy.INFO)
-
-        # Get the actual node name in case it is set in the launch file
-        self.name = rospy.get_name()
-
-        # Cleanup when termniating the node
-        rospy.on_shutdown(self.shutdown)
-
-        self.port = rospy.get_param("~port", "/dev/ttyACM0")
-        self.baud = int(rospy.get_param("~baud", 57600))
-        self.timeout = rospy.get_param("~timeout", 0.5)
-        self.base_frame = rospy.get_param("~base_frame", 'base_link')
-        self.camera_frame = rospy.get_param("~camera_frame", 'depth_camera_link')
-        self.motors_reversed = rospy.get_param("~motors_reversed", False)
-        # Overall loop rate: should be faster than fastest sensor rate
-        self.rate = int(rospy.get_param("~rate", 50))
-        r = rospy.Rate(self.rate)
-
-        self.use_base_controller = rospy.get_param("~use_base_controller", False)
-
-        # Initialize a Twist message
-        self.cmd_vel = Twist()
-
-        # A cmd_vel publisher so we can stop the robot when shutting down
-        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
-
-        # Initialize the controlller
-        self.controller = Arduino(self.port, self.baud, self.timeout, self.motors_reversed)
-
-        # Make the connection
-        self.controller.connect()
-
-        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
-
-        # Reserve a thread lock
-        mutex = thread.allocate_lock()
-
-        # Initialize the base controller if used
-        if self.use_base_controller:
-            self.qt_pi_BaseController = BaseController(self.controller, self.base_frame, self.camera_frame, self.name + "_base_controller")
-        
-        # Start polling the sensors and base controller
-        while not rospy.is_shutdown():
-            if self.use_base_controller:
-                mutex.acquire()
-                self.qt_pi_BaseController.poll()
-                mutex.release()
-        
-            r.sleep()
-
-    def shutdown(self):
-        rospy.loginfo("Shutting down Arduino Node...")
-
-        # Stop the robot
-        try:
-            rospy.loginfo("Stopping the robot...")
-            self.cmd_vel_pub.Publish(Twist())
-            rospy.sleep(2)
-        except:
-            pass
-
-        # Close the serial port
-        try:
-            self.controller.close()
-        except:
-            pass
-        finally:
-            rospy.loginfo("Serial port closed.")
-            os._exit(0)
-
-if __name__ == '__main__':
-    try:
-        qt_pi_Arduino = ArduinoROS()
-    except SerialException:
-        rospy.logerr("Serial exception trying to open port.")
-        os._exit(0)

qt_pi/qt_pi_eeg/CMakeLists.txt

@@ -1,31 +1,14 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(qt_pi)
+project(qt_pi_eeg)
 
 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)
@@ -39,7 +22,6 @@ 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}
 )
@@ -48,21 +30,7 @@ include_directories(include
 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)

qt_pi/qt_pi_eeg/package.xml

@@ -0,0 +1,11 @@
+<package format="2">
+    <name>qt_pi_eeg</name>
+    <version>0.0.0</version>
+    <description>EEG processing package of qt_pi</description>
+    <maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
+    <license>MIT</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>roscpp</build_depend>
+</package>

qt_pi/qt_pi_embedded/CMakeLists.txt.tmp

@@ -0,0 +1,109 @@
+# I target a recent cmake, it shouldn't be a problem on a dev machine
+cmake_minimum_required(VERSION 2.8.3)
+# Project name
+project(qt_pi_embedded_firmware)
+
+## AVR Chip Configuration
+set(F_CPU 16000000UL)
+# CPU, you can find the list here:
+# https://gcc.gnu.org/onlinedocs/gcc/AVR-Options.html
+set(MCU atmega328p)
+# Default Baudrate for UART, read avr include/util/setbaud.h for usage
+set(BAUDRATE 115200)
+# The programmer to use, read avrdude manual for list
+set(PROG_TYPE arduino)
+
+set(UPLOAD_BAUD 57600)
+# AVR Fuses, must be in concordance with your hardware and F_CPU
+# http://eleccelerator.com/fusecalc/fusecalc.php?chip=atmega328p
+set(E_FUSE 0xfd)
+set(H_FUSE 0xda)
+set(L_FUSE 0xfd)
+set(LOCK_BIT 0xff)
+
+
+# Use AVR GCC toolchain
+set(CMAKE_SYSTEM_NAME Generic)
+set(CMAKE_CXX_COMPILER avr-g++)
+set(CMAKE_C_COMPILER avr-gcc)
+set(CMAKE_ASM_COMPILER avr-gcc)
+set(ARDUINO_SDK /home/yikes/arduino-1.8.13)
+
+# Pass defines to compiler
+add_definitions(
+    -DF_CPU=${F_CPU}
+    -DBAUD=${BAUD}
+    -DARDUINO=10813
+    -DARDUINO_AVR_NANO
+    -DARDUINO_ARCH_AVR
+)
+# mmcu MUST be passed to bot the compiler and linker, this handle the linker
+set(CMAKE_EXE_LINKER_FLAGS -mmcu=${MCU})
+
+add_compile_options(
+    -mmcu=${MCU} # MCU
+    -std=gnu++11 # C99 standard
+    -Os # optimize
+    -Wall # enable warnings
+    -Wno-main
+    -Wundef
+    -pedantic
+    -Wstrict-prototypes
+    -Werror
+    -Wfatal-errors
+    -Wl,--relax,--gc-sections
+    -g
+    -gdwarf-2
+    -funsigned-char # a few optimizations
+    -funsigned-bitfields
+    -fpack-struct
+    -fshort-enums
+    -ffunction-sections
+    -fdata-sections
+    -fno-split-wide-types
+    -fno-tree-scev-cprop
+    -c
+    -MMD
+    -w
+    -fno-exceptions
+    -fno-threadsafe-statics
+    -flto
+
+)
+
+file(GLOB SRC_FILES "src/*.cpp") # Load all files in src folder
+file(GLOB_RECURSE ARDUINO_SDK_FILES 
+    ${ARDUINO_SDK}/hardware/arduino/avr/variants/*
+)
+
+file(GLOB_RECURSE ARDUINO_SDK_LIBS 
+    ${ARDUINO_SDK}/hardware/arduino/avr/libraries/*
+    )
+message(STATUS "Hello" ${ARDUINO_SDK_FILES})
+
+include_directories(
+    include
+    ${ARDUINO_SDK}/hardware/arduino/avr/cores/arduino
+    ${ARDUINO_SDK_FILES}
+    ${ARDUINO_SDK_LIBS}
+    ${ARDUINO_EXT_LIB}
+)
+
+# Create one target
+add_executable(${PROJECT_NAME}_exe ${ARDUINO_SDK}/hardware/arduino/avr/cores/arduino/main.cpp)
+
+# Rename the output to .elf as we will create multiple files
+set_target_properties(${PRODUCT_NAME} PROPERTIES OUTPUT_NAME ${PRODUCT_NAME}.elf)
+
+# Strip binary for upload
+#add_custom_target(strip ALL avr-strip ${PRODUCT_NAME}.elf DEPENDS ${PRODUCT_NAME})
+
+# Transform binary into hex file, we ignore the eeprom segments in the step
+#add_custom_target(hex ALL avr-objcopy -R .eeprom -O ihex ${PRODUCT_NAME}.elf ${PRODUCT_NAME}.hex DEPENDS strip)
+# Transform binary into hex file, this is the eeprom part (empty if you don't
+# use eeprom static variables)
+#add_custom_target(eeprom avr-objcopy -j .eeprom  --set-section-flags=.eeprom="alloc,load"  --change-section-lma .eeprom=0 -O ihex ${PRODUCT_NAME}.elf ${PRODUCT_NAME}.eep DEPENDS strip)
+
+# Upload the firmware with avrdude
+#add_custom_target(upload avrdude -C${ARDUINO_SDK}/hardware/tools/avr/etc/avrdude.conf -c${PROG_TYPE} -v -V -p${MCU} -D -b${UPLOAD_BAUD} -Uflash:w:${PRODUCT_NAME}.hex:i DEPENDS hex)
+

qt_pi/qt_pi_embedded/src/embedded_firmware/comm.cpp

@@ -0,0 +1,374 @@
+#include "qt_pi.h"
+/*
+#define  WAITING_FOR_NEW_COM    0
+#define  FIRST_BYTE_VERIFIED    1
+#define  SECOND_BYTE_VERIFIED   2
+#define  LENGTH_INPUT           3
+#define  READING_PAYLOAD        4
+#define  READING_COMPLETE       5
+#define  COM_RECEIVED           6
+*/
+enum COMM_STATE
+{
+  WAITING_FOR_NEW_COM,
+  FIRST_BYTE_VERIFIED,
+  SECOND_BYTE_VERIFIED,
+  LENGTH_INPUT,
+  READING_PAYLOAD,
+  READING_COMPLETE,
+  COM_RECEIVED
+};
+
+int curState = WAITING_FOR_NEW_COM;
+
+int com_len;
+
+uint8_t in_buf[buf_size];
+int in_buf_i;
+
+uint8_t ser_write_buf[buf_size];
+uint8_t com_recv[buf_size];
+
+int
+init_comm ()
+{
+
+  ser_write_buf[MB1P] = magic_start1;
+  ser_write_buf[MB2P] = magic_start2;
+
+  Serial.begin (BAUDRATE);
+  Serial.setTimeout (0);
+
+  resetComm ();
+
+  curState = WAITING_FOR_NEW_COM;
+}
+
+int
+sendCommand (int16_t len)
+{
+  if ((len + net_extra_len) > buf_size)
+    return -1;
+
+  memcpy (ser_write_buf + start_magic_len, &len, len_len);
+
+  ser_write_buf[preamble_len + len] = magic_end;
+
+  return (Serial.write (ser_write_buf, len + net_extra_len));
+}
+
+int
+sendData (int len, uint8_t *buf)
+{
+
+  if ((len + net_extra_len) > buf_size)
+    return -1;
+
+  memcpy (ser_write_buf + preamble_len, buf, len);
+
+  return sendCommand (len);
+}
+
+int
+runInpCommand ()
+{
+
+  int data_len = com_len - com_preamble_len;
+
+  switch (com_recv[COMB])
+    {
+
+    case GET_BAUDRATE:
+
+      if (data_len != GET_BAUDRATE_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      memcpy (ser_write_buf + DATASTRBP, &BAUDRATE, 4);
+
+      data_len = 4;
+
+      break;
+
+    case GET_ENCODERS:
+      {
+
+        if (data_len != GET_ENCODERS_DATA_LEN)
+          {
+            data_len = -1;
+            break;
+          }
+
+        long l = readEncoder (LEFT_WHEEL);
+        long r = readEncoder (RIGHT_WHEEL);
+
+        memcpy (ser_write_buf + DATASTRBP, &l, 4);
+        memcpy (ser_write_buf + DATASTRBP + 4, &r, 4);
+
+        data_len = 8;
+
+        break;
+      }
+
+    case RESET_ENCODERS:
+
+      if (data_len != RESET_ENCODERS_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      resetEncoders ();
+      resetPID ();
+
+      ser_write_buf[DATASTRBP] = 1;
+      data_len = 1;
+
+      break;
+
+    case SET_SPEEDS:
+
+      if (data_len != SET_SPEEDS_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      move_cmd_timeout_reset ();
+
+      if (com_recv[DATASTRB] == 0 && com_recv[DATASTRB + 1] == 0)
+        {
+
+          setMotorSpeeds (0, 0);
+
+          resetPID ();
+
+          setMoving (false);
+        }
+      else
+        {
+          setMoving (true);
+        }
+
+      setTicksPerFrame (com_recv + DATASTRB);
+
+      ser_write_buf[DATASTRBP] = 1;
+      data_len = 1;
+
+      break;
+
+    case SET_PID_L:
+
+      if (data_len != SET_PID_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      setPID_L (com_recv + DATASTRB);
+
+      ser_write_buf[DATASTRBP] = 1;
+      data_len = 1;
+
+      break;
+
+    case SET_PID_R:
+
+      if (data_len != SET_PID_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      setPID_R (com_recv + DATASTRB);
+
+      ser_write_buf[DATASTRBP] = 1;
+      data_len = 1;
+
+      break;
+
+    case GET_PID_P:
+
+      if (data_len != GET_PID_P_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      getPID (ser_write_buf + DATASTRBP);
+
+      data_len = 8;
+
+      break;
+
+    case GET_BATTERY_LVL:
+
+      if (data_len != GET_BATTERY_LVL_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      getBattLvl (ser_write_buf + DATASTRBP);
+
+      data_len = 2;
+
+      break;
+
+    case EN_IMU:
+
+      if (data_len != EN_IMU_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      enable_imu (com_recv[DATASTRB]);
+
+      ser_write_buf[DATASTRBP] = 1;
+      data_len = 1;
+
+      break;
+
+    case EN_ODOM:
+
+      if (data_len != EN_ODOM_DATA_LEN)
+        {
+          data_len = -1;
+          break;
+        }
+
+      enable_odom (com_recv[DATASTRB]);
+
+      ser_write_buf[DATASTRBP] = 1;
+      data_len = 1;
+
+      break;
+
+    default:
+
+      ser_write_buf[DATASTRBP] = com_recv[COMB];
+      com_recv[COMB] = INVALID;
+      data_len = 1;
+
+      break;
+    }
+
+  if (data_len == -1)
+    {
+      ser_write_buf[COMBP] = INVALID_LENGTH;
+      ser_write_buf[CIDBP] = com_recv[CIDB];
+      ser_write_buf[DATASTRBP] = com_recv[COMB];
+      data_len = 1;
+    }
+  else
+    {
+      ser_write_buf[COMBP] = com_recv[COMB];
+      ser_write_buf[CIDBP] = com_recv[CIDB];
+    }
+
+  sendCommand (data_len + com_preamble_len);
+}
+
+void
+resetComm ()
+{
+  com_len = 1;
+  in_buf_i = 0;
+}
+
+uint8_t temp_buf[64];
+void
+serCommTimeout ()
+{
+  Serial.readBytes (temp_buf, 64);
+}
+
+void
+proc_serial ()
+{
+
+  if (!Serial.available ())
+    return;
+
+  move_cmd_timeout_reset ();
+
+  switch (curState)
+    {
+
+    case WAITING_FOR_NEW_COM:
+
+      if (Serial.read () == magic_start1)
+        {
+          curState = FIRST_BYTE_VERIFIED;
+        }
+    
+      break;
+
+    case FIRST_BYTE_VERIFIED:
+
+      if (Serial.read () == magic_start2)
+        {
+          curState = SECOND_BYTE_VERIFIED;
+        }
+      else
+        {
+          curState = WAITING_FOR_NEW_COM;
+        }
+
+      break;
+
+    case SECOND_BYTE_VERIFIED:
+
+      com_len = Serial.read ();
+      curState = LENGTH_INPUT;
+
+      break;
+
+    case LENGTH_INPUT:
+
+      com_len = com_len | (Serial.read () << 8);
+
+      if (com_len < 20)
+        {
+          curState = READING_PAYLOAD;
+        }
+      else
+        {
+          curState = WAITING_FOR_NEW_COM;
+        }
+
+      break;
+
+    case READING_PAYLOAD:
+
+      in_buf_i += Serial.readBytes (&in_buf[in_buf_i], com_len - in_buf_i);
+
+      if (in_buf_i == com_len)
+        {
+          curState = READING_COMPLETE;
+        }
+      break;
+
+    case READING_COMPLETE:
+
+      if (Serial.read () == magic_end)
+        {
+
+          memcpy (com_recv, in_buf, com_len);
+
+          runInpCommand ();
+
+          curState = WAITING_FOR_NEW_COM;
+        }
+
+      resetComm ();
+
+      break;
+    }
+
+  if ((Serial.available () > 0) && curState != WAITING_FOR_NEW_COM)
+    proc_serial ();
+}

qt_pi/qt_pi_embedded/src/embedded_firmware/commands.h

@@ -0,0 +1,35 @@
+#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_DATA_LEN 0x01
+#define GET_ENCODERS_DATA_LEN 0x01
+#define SET_SPEEDS_DATA_LEN 0x03
+#define RESET_ENCODERS_DATA_LEN 0x01
+#define SET_PID_DATA_LEN 0x04
+#define GET_PID_P_DATA_LEN 0x01
+#define GET_BATTERY_LVL_DATA_LEN 0x01
+#define EN_IMU_DATA_LEN 0x01
+#define EN_ODOM_DATA_LEN 0x01
+#define ODOM_DATA_LEN 0x08
+#define IMU_DATA_LEN 0x08
+
+#define LEFT_WHEEL 0x00
+#define RIGHT_WHEEL 0x01
+
+#endif

qt_pi/qt_pi_embedded/src/embedded_firmware/diff_controller.cpp

@@ -29,14 +29,14 @@ unsigned char moving = 0;
 void resetPID(){
   
   leftPID.TargetTicksPerFrame = 0.0;
-  leftPID.Encoder = readEncoder(LEFT);
+  leftPID.Encoder = readEncoder(LEFT_WHEEL);
   leftPID.PrevEnc = leftPID.Encoder;
   leftPID.output = 0;
   leftPID.PrevInput = 0;
   leftPID.ITerm = 0;
   
   rightPID.TargetTicksPerFrame = 0.0;
-  rightPID.Encoder = readEncoder(RIGHT);
+  rightPID.Encoder = readEncoder(RIGHT_WHEEL);
   rightPID.PrevEnc = rightPID.Encoder;
   rightPID.output = 0;
   rightPID.PrevInput = 0;
@@ -85,8 +85,8 @@ void doPID(SetPointInfo * p) {
 
 void updatePID() {
 
-  leftPID.Encoder = readEncoder(LEFT);
+  leftPID.Encoder = readEncoder(LEFT_WHEEL);
-  rightPID.Encoder = readEncoder(RIGHT);
+  rightPID.Encoder = readEncoder(RIGHT_WHEEL);
 
   if (!moving){
     

qt_pi/qt_pi_embedded/src/embedded_firmware/embedded_firmware.ino

@@ -0,0 +1,90 @@
+#include "qt_pi.h"
+
+#define _TASK_SLEEP_ON_IDLE_RUN
+#define _TASK_INLINE
+#define _TASK_TIMEOUT
+
+#include <TaskScheduler.h>
+
+Task *batt_mon;
+Task *diff_control;
+Task *move_cmd_timeout;
+Task *odom_out;
+Task *imu_out;
+Task *ser_comm_timeout;
+
+Scheduler ts;
+
+void setup() {
+
+  init_comm();
+
+  init_enc();
+
+  initMotorController();
+
+  batt_mon = new Task(BATT_INTERVAL, TASK_FOREVER, &updateBattLevel, &ts, true,
+                      &initBatt, NULL);
+  diff_control = new Task(PID_INTERVAL, TASK_FOREVER, &updatePID, &ts, true,
+                          &resetPID, NULL);
+  move_cmd_timeout =
+      new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, true, NULL, NULL);
+  odom_out =
+      new Task(ODOM_INTERVAL, TASK_FOREVER, &sendOdom, &ts, false, NULL, NULL);
+  imu_out = new Task(IMU_INTERVAL, TASK_FOREVER, &sendIMU, &ts, false, &initIMU,
+                     NULL);
+  ser_comm_timeout =
+      new Task(AUTO_STOP_INTERVAL, 1, &serCommTimeout, &ts, true, NULL, NULL);
+}
+
+void loop() {
+
+  ts.execute();
+  
+  proc_serial();
+  
+}
+
+void move_cmd_timeout_reset() {
+  //  move_cmd_timeout->resetTimeout();
+}
+
+void enable_odom(bool a) {
+  switch (a) {
+  case 0:
+    switch (odom_out->isEnabled()) {
+    case 1:
+      odom_out->disable();
+      break;
+    }
+    break;
+
+  case 1:
+    switch (odom_out->isEnabled()) {
+    case 0:
+      odom_out->enable();
+      break;
+    }
+    break;
+  }
+}
+
+void enable_imu(bool a) {
+  switch (a) {
+  case 0:
+    switch (imu_out->isEnabled()) {
+    case 1:
+      imu_out->disable();
+      break;
+    }
+    break;
+
+  case 1:
+    switch (imu_out->isEnabled()) {
+    case 0:
+      imu_out->enable();
+      break;
+    }
+    break;
+  }
+}

qt_pi/qt_pi_embedded/src/embedded_firmware/encoder_driver.cpp

@@ -51,7 +51,7 @@ void init_enc(){
 
 long readEncoder(int i) {
   
-  if (i == LEFT) 
+  if (i == LEFT_WHEEL) 
       return left_enc_pos;
     
     else 
@@ -61,7 +61,7 @@ long readEncoder(int i) {
 
   void resetEncoder(int i) {
     
-    if (i == LEFT){
+    if (i == LEFT_WHEEL){
       left_enc_pos=0L;
       return;
     } 
@@ -74,8 +74,8 @@ long readEncoder(int i) {
 
 void resetEncoders() {
   
-  resetEncoder(LEFT);
+  resetEncoder(LEFT_WHEEL);
-  resetEncoder(RIGHT);
+  resetEncoder(RIGHT_WHEEL);
 
 }
 

qt_pi/qt_pi_embedded/src/embedded_firmware/motor_driver.cpp

@@ -32,7 +32,7 @@ void setMotorSpeed(int i, int spd) {
   { 
     switch(i){
       
-      case LEFT:
+      case LEFT_WHEEL:
         
         switch(reverse)
         {
@@ -47,7 +47,7 @@ void setMotorSpeed(int i, int spd) {
         }
         break;
         
-      case RIGHT:
+      case RIGHT_WHEEL:
         switch(reverse)
         {
             case 0:  
@@ -66,7 +66,7 @@ void setMotorSpeed(int i, int spd) {
   
 void setMotorSpeeds(int leftSpeed, int rightSpeed) {
   
-  setMotorSpeed(LEFT, leftSpeed);
+  setMotorSpeed(LEFT_WHEEL, leftSpeed);
-  setMotorSpeed(RIGHT, rightSpeed);
+  setMotorSpeed(RIGHT_WHEEL, rightSpeed);
   
 }

qt_pi/qt_pi_embedded/src/embedded_firmware/payload_specs.h

@@ -0,0 +1,33 @@
+#ifndef PAYLOAD_SPECS_H
+#define PAYLOAD_SPECS_H
+
+#define buf_size 32
+
+#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;
+
+const unsigned char magic_start1 = 0xEE;
+const unsigned char magic_start2 = 0xAE;
+const unsigned char magic_end = 0xFC;
+
+#endif

qt_pi/qt_pi_embedded/src/embedded_firmware/qt_pi.h

@@ -6,12 +6,12 @@
 #include "commands.h"
 #include "payload_specs.h"
 
-#include "imu.h"
-#include "comm.h"
 #include "batt.h"
-#include "motor_driver.h"
+#include "comm.h"
-#include "encoder_driver.h"
 #include "diff_controller.h"
+#include "encoder_driver.h"
+#include "imu.h"
+#include "motor_driver.h"
 
 extern void move_cmd_timeout_reset();
 extern void enable_imu(bool);

qt_pi/qt_pi_gui/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(qt_pi_gui)
+
+set(CMAKE_CXX_FLAGS "-std=c++0x")
+#-fpermissive
+#ros setup
+find_package(catkin REQUIRED COMPONENTS
+qt_build
+roscpp
+roslib
+qt_pi_msgs
+)
+
+#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)
+
+include_directories(include
+    ${catkin_INCLUDE_DIRS}
+)
+
+#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)
+
+#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)

qt_pi/qt_pi_gui/package.xml

@@ -1,18 +1,17 @@
 <package format="2">
-    <name>qt_pi</name>
+    <name>qt_pi_gui</name>
     <version>0.0.0</version>
-    <description>The qt_pi package</description>
+    <description>GUI package of qt_pi</description>
-    <maintainer email="rishabhkundu13@gmail.com">Rishabh Kundu</maintainer>
+    <maintainer email="rishabh_kundu@rediffmail.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>
+</package>
-    <export></export>
-</package>

qt_pi/qt_pi_msgs/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(qt_pi_msgs)
+
+find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs sensor_msgs nav_msgs message_generation)
+
+add_message_files(
+    FILES
+    Encoder.msg
+    PID.msg
+)
+
+add_service_files(
+    FILES
+    GetBatteryInfo.srv
+    GetBaudRate.srv
+    GetEncoderData.srv
+    GetPIDData.srv
+    SetPIDData.srv
+)
+
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+    geometry_msgs
+    sensor_msgs
+    nav_msgs
+)
+
+catkin_package(
+    CATKIN_DEPENDS
+    message_runtime
+    std_msgs
+    geometry_msgs
+    sensor_msgs
+    nav_msgs
+)

qt_pi/qt_pi_msgs/msg/Encoder.msg

@@ -0,0 +1,2 @@
+int64 LEFT
+int64 RIGHT

qt_pi/qt_pi_msgs/msg/PID.msg

@@ -0,0 +1,8 @@
+uint8 lkp
+uint8 lkd
+uint8 lki
+uint8 lko
+uint8 rkp
+uint8 rkd
+uint8 rki
+uint8 rko

qt_pi/qt_pi_msgs/package.xml

@@ -0,0 +1,19 @@
+<package format="2">
+  <name>qt_pi_msgs</name>
+  <version>0.0.0</version>
+  <description>Custom msgs and srvs package for qt_pi</description>
+  <maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+</package>

qt_pi/qt_pi_msgs/srv/GetBatteryInfo.srv

@@ -0,0 +1,2 @@
+---
+float32 volts

qt_pi/qt_pi_msgs/srv/GetBaudRate.srv

@@ -0,0 +1,2 @@
+---
+uint32 BAUDRATE

qt_pi/qt_pi_msgs/srv/GetEncoderData.srv

@@ -0,0 +1,2 @@
+---
+qt_pi_msgs/Encoder data

qt_pi/qt_pi_msgs/srv/GetPIDData.srv

@@ -0,0 +1,2 @@
+---
+qt_pi_msgs/PID data

qt_pi/qt_pi_msgs/srv/SetPIDData.srv

@@ -0,0 +1,2 @@
+qt_pi_msgs/PID data
+---

qt_pi/qt_pi_serial/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(qt_pi_serial)
+
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    qt_pi_msgs
+    std_msgs
+    sensor_msgs
+    nav_msgs
+    std_srvs
+)
+
+catkin_package()
+
+set(SRCS src/ros_node.cpp src/serial_driver.cpp)
+set(HPP include/qt_pi_serial/commands.h include/qt_pi_serial/ros_node.h include/qt_pi_serial/payload_specs.h include/qt_pi_serial/serial_driver.h)
+
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+)
+
+add_library(serial_comm ${SRCS})
+add_dependencies(serial_comm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(serial_comm ${catkin_LIBRARIES})
+
+add_executable(serial_node src/main.cpp)
+target_link_libraries(serial_node serial_comm)
+
+add_executable(serial_comm_test tests/comm_test.cpp)
+target_link_libraries(serial_comm_test serial_comm)
+

qt_pi/qt_pi_serial/include/qt_pi_serial/commands.h

@@ -0,0 +1,35 @@
+#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_DATA_LEN 0x01
+#define GET_ENCODERS_DATA_LEN 0x01
+#define SET_SPEEDS_DATA_LEN 0x03
+#define RESET_ENCODERS_DATA_LEN 0x01
+#define SET_PID_DATA_LEN 0x04
+#define GET_PID_P_DATA_LEN 0x01
+#define GET_BATTERY_LVL_DATA_LEN 0x01
+#define EN_IMU_DATA_LEN 0x01
+#define EN_ODOM_DATA_LEN 0x01
+#define ODOM_DATA_LEN 0x08
+#define IMU_DATA_LEN 0x08
+
+#define LEFT_WHEEL 0x00
+#define RIGHT_WHEEL 0x01
+
+#endif

qt_pi/qt_pi_serial/include/qt_pi_serial/payload_specs.h

@@ -0,0 +1,33 @@
+#ifndef PAYLOAD_SPECS_H
+#define PAYLOAD_SPECS_H
+
+#define buf_size 32
+
+#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;
+
+const unsigned char magic_start1 = 0xEE;
+const unsigned char magic_start2 = 0xAE;
+const unsigned char magic_end = 0xFC;
+
+#endif

qt_pi/qt_pi_serial/include/qt_pi_serial/ros_node.h

@@ -0,0 +1,46 @@
+#ifndef ROS_NODE_H
+#define ROS_NODE_H
+
+#include <ros/ros.h>
+
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/UInt32.h>
+
+#include "qt_pi_msgs/GetBatteryInfo.h"
+#include "qt_pi_msgs/GetBaudRate.h"
+#include "qt_pi_msgs/GetEncoderData.h"
+#include "qt_pi_msgs/GetPIDData.h"
+#include "qt_pi_msgs/SetPIDData.h"
+#include "serial_driver.h"
+#include "std_srvs/SetBool.h"
+#include "std_srvs/Trigger.h"
+
+ros::Publisher imu_pub;
+ros::Publisher odom_pub;
+ros::ServiceServer get_encoder;
+ros::ServiceServer get_batt_state;
+ros::ServiceServer get_pid_val;
+ros::ServiceServer get_baudrate;
+ros::ServiceServer set_pid_val;
+ros::ServiceServer en_imu;
+ros::ServiceServer en_odom;
+ros::ServiceServer reset_odom;
+
+bool GET_ENC_SRV_CB(qt_pi_msgs::GetEncoderData::Request &req,
+                    qt_pi_msgs::GetEncoderData::Response &res);
+bool GET_BATT_SRV_CB(qt_pi_msgs::GetBatteryInfo::Request &req,
+                     qt_pi_msgs::GetBatteryInfo::Response &res);
+bool GET_PID_SRV_CB(qt_pi_msgs::GetPIDData::Request &req,
+                    qt_pi_msgs::GetPIDData::Response &res);
+bool GET_BAUD_SRV_CB(qt_pi_msgs::GetBaudRate::Request &req,
+                     qt_pi_msgs::GetBaudRate::Response &res);
+bool SET_PID_SRV_CB(qt_pi_msgs::SetPIDData::Request &req,
+                    qt_pi_msgs::SetPIDData::Response &res);
+bool EN_IMU_SRV_CB(std_srvs::SetBool::Request &req,
+                   std_srvs::SetBool::Response &res);
+bool EN_ODOM_SRV_CB(std_srvs::SetBool::Request &req,
+                    std_srvs::SetBool::Response &res);
+bool RESET_ODOM_SRV_CB(std_srvs::Trigger::Request &req,
+                       std_srvs::Trigger::Response &res);
+#endif

qt_pi/qt_pi_serial/include/qt_pi_serial/serial_driver.h

@@ -0,0 +1,27 @@
+#ifndef SERIAL_DRIVER_H
+#define SERIAL_DRIVER_H
+
+#include "commands.h"
+#include "payload_specs.h"
+#include <errno.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include <stdint.h>
+#include <string.h>
+#include <termios.h>
+#include <time.h>
+#include <unistd.h>
+
+enum com_state { IDLE, COM_SENT, COM_RECEIVED };
+
+extern unsigned char com_buf[buf_size];
+extern int com_buf_len;
+
+extern int init_comm(const char *);
+extern void close_comm();
+extern int sendCom();
+
+extern void IMU_DATA_PUBLISH(unsigned char *);
+extern void ODOM_DATA_PUBLISH(unsigned char *);
+
+#endif

qt_pi/qt_pi_serial/package.xml

@@ -0,0 +1,25 @@
+<package format="2">
+    <name>qt_pi_serial</name>
+    <version>0.0.0</version>
+    <description>Serial communication with microcontroller package of qt_pi</description>
+    <maintainer email="rishabh_kundu@rediffmail.com">Rishabh Kundu</maintainer>
+    <license>MIT</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>roscpp</build_depend>
+    <build_depend>qt_pi_msgs</build_depend>
+    <build_depend>nav_msgs</build_depend>
+    <build_depend>geometry_msgs</build_depend>
+    <build_depend>sensor_msgs</build_depend>
+    <build_depend>std_msgs</build_depend>
+    <build_depend>std_srvs</build_depend>
+
+    <exec_depend>roscpp</exec_depend>
+    <exec_depend>qt_pi_msgs</exec_depend>
+    <exec_depend>nav_msgs</exec_depend>
+    <exec_depend>geometry_msgs</exec_depend>
+    <exec_depend>sensor_msgs</exec_depend>
+    <exec_depend>std_msgs</exec_depend>
+    <exec_depend>std_srvs</exec_depend>
+</package>

qt_pi/qt_pi_serial/src/main.cpp

@@ -0,0 +1,46 @@
+#include "qt_pi_serial/ros_node.h"
+#include <csignal>
+
+void sighandler(int signum) {
+  printf("Caught signal %d, Exiting after thread completion\n", signum);
+  close_comm();
+  printf("COMM CLOSED\n");
+  ros::shutdown();
+  printf("shutting down\n");
+}
+
+int main(int argc, char **argv) {
+
+  ros::init(argc, argv, "serial_driver");
+
+  ros::NodeHandle n;
+  ROS_INFO("REACHED1\n");
+
+  if (init_comm("/dev/ttyUSB0") < 0) {
+    ROS_FATAL("Unable to open port");
+    exit(0);
+  }
+  imu_pub = n.advertise<sensor_msgs::Imu>("imu", 1);
+  ROS_INFO("REACHED2\n");
+  odom_pub = n.advertise<nav_msgs::Odometry>("odom", 1);
+  ROS_INFO("REACHED3\n");
+  get_encoder = n.advertiseService("get_encoder_state", GET_ENC_SRV_CB);
+  ROS_INFO("REACHED4\n");
+  get_batt_state = n.advertiseService("get_battery_state", GET_BATT_SRV_CB);
+  ROS_INFO("REACHED5\n");
+  get_pid_val = n.advertiseService("get_pid_values", GET_PID_SRV_CB);
+  ROS_INFO("REACHED6\n");
+  get_baudrate = n.advertiseService("get_baudrate", GET_BAUD_SRV_CB);
+  ROS_INFO("REACHED7\n");
+  set_pid_val = n.advertiseService("set_pid_values", SET_PID_SRV_CB);
+  ROS_INFO("REACHED8\n");
+  en_imu = n.advertiseService("set_imu_state", EN_IMU_SRV_CB);
+  ROS_INFO("REACHED9\n");
+  en_odom = n.advertiseService("set_odom_state", EN_ODOM_SRV_CB);
+  ROS_INFO("REACHED10\n");
+  reset_odom = n.advertiseService("reset_odom", RESET_ODOM_SRV_CB);
+  ROS_INFO("REACHED11\n");
+
+  signal(SIGINT, sighandler);
+  ros::spin();
+}

qt_pi/qt_pi_serial/src/ros_node.cpp

@@ -0,0 +1,194 @@
+#include "qt_pi_serial/ros_node.h"
+
+sensor_msgs::Imu data;
+
+bool GET_ENC_SRV_CB(qt_pi_msgs::GetEncoderData::Request &req,
+                    qt_pi_msgs::GetEncoderData::Response &res) {
+
+  com_buf[COMB] = GET_ENCODERS;
+  com_buf[CIDB] = 1;
+  com_buf[DATASTRB] = 1;
+
+  com_buf_len = 3;
+
+  int ret = sendCom();
+
+  if (ret)
+    return false;
+
+  memcpy(&res.data.LEFT, com_buf + DATASTRB, 4);
+  memcpy(&res.data.RIGHT, com_buf + DATASTRB, 4);
+
+  return true;
+}
+
+bool GET_BATT_SRV_CB(qt_pi_msgs::GetBatteryInfo::Request &req,
+                     qt_pi_msgs::GetBatteryInfo::Response &res) {
+  com_buf[COMB] = GET_BATTERY_LVL;
+  com_buf[CIDB] = 1;
+  com_buf[DATASTRB] = 1;
+
+  com_buf_len = 3;
+
+  int ret = sendCom();
+
+  if (ret)
+    return false;
+
+  uint16_t volts;
+
+  memcpy(&volts, com_buf + DATASTRB, 2);
+
+  res.volts = volts / 100.0;
+}
+
+bool GET_PID_SRV_CB(qt_pi_msgs::GetPIDData::Request &req,
+                    qt_pi_msgs::GetPIDData::Response &res) {
+
+  com_buf[COMB] = GET_PID_P;
+  com_buf[CIDB] = 1;
+  com_buf[DATASTRB] = 1;
+
+  com_buf_len = 3;
+
+  int ret = sendCom();
+
+  if (ret)
+    return false;
+
+  res.data.lkp = com_buf[DATASTRB];
+  res.data.lkd = com_buf[DATASTRB + 1];
+  res.data.lki = com_buf[DATASTRB + 2];
+  res.data.lko = com_buf[DATASTRB + 3];
+  res.data.rkp = com_buf[DATASTRB + 4];
+  res.data.rkd = com_buf[DATASTRB + 5];
+  res.data.rki = com_buf[DATASTRB + 6];
+  res.data.rko = com_buf[DATASTRB + 7];
+
+  return true;
+}
+
+bool GET_BAUD_SRV_CB(qt_pi_msgs::GetBaudRate::Request &req,
+                     qt_pi_msgs::GetBaudRate::Response &res) {
+  com_buf[COMB] = GET_BAUDRATE;
+  com_buf[CIDB] = 1;
+  com_buf[DATASTRB] = 1;
+
+  com_buf_len = 3;
+  ROS_INFO("CALLED\n");
+  int ret = sendCom();
+
+  ROS_INFO("CALLED\n");
+  if (ret)
+    return false;
+
+  ROS_INFO("CALLED\n");
+  memcpy(&(res.BAUDRATE), com_buf + DATASTRB, 4);
+
+  ROS_INFO("CALLED\n");
+  return true;
+}
+
+bool SET_PID_SRV_CB(qt_pi_msgs::SetPIDData::Request &req,
+                    qt_pi_msgs::SetPIDData::Response &res) {
+  com_buf[COMB] = GET_PID_P;
+  com_buf[CIDB] = 1;
+
+  com_buf[DATASTRB] = req.data.lkp;
+  com_buf[DATASTRB + 1] = req.data.lkd;
+  com_buf[DATASTRB + 2] = req.data.lki;
+  com_buf[DATASTRB + 3] = req.data.lko;
+  com_buf[DATASTRB + 4] = req.data.rkp;
+  com_buf[DATASTRB + 5] = req.data.rkd;
+  com_buf[DATASTRB + 6] = req.data.rki;
+  com_buf[DATASTRB + 7] = req.data.rko;
+
+  com_buf_len = 10;
+
+  int ret = sendCom();
+
+  if (ret)
+    return false;
+
+  return true;
+}
+bool EN_IMU_SRV_CB(std_srvs::SetBool::Request &req,
+                   std_srvs::SetBool::Response &res) {
+  com_buf[COMB] = EN_IMU;
+  com_buf[CIDB] = 1;
+  com_buf[DATASTRB] = req.data;
+
+  com_buf_len = 3;
+
+  res.success = 0;
+
+  int ret = sendCom();
+
+  if (ret)
+    return false;
+
+  res.success = 1;
+
+  return true;
+}
+bool EN_ODOM_SRV_CB(std_srvs::SetBool::Request &req,
+                    std_srvs::SetBool::Response &res) {
+  com_buf[COMB] = EN_ODOM;
+  com_buf[CIDB] = 1;
+  com_buf[DATASTRB] = req.data;
+
+  com_buf_len = 3;
+
+  res.success = 0;
+
+  int ret = sendCom();
+
+  if (ret)
+    return false;
+
+  res.success = 1;
+
+  return true;
+}
+bool RESET_ODOM_SRV_CB(std_srvs::Trigger::Request &req,
+                       std_srvs::Trigger::Response &res) {
+  com_buf[COMB] = RESET_ENCODERS;
+  com_buf[CIDB] = 1;
+  com_buf[DATASTRB] = 1;
+
+  com_buf_len = 3;
+
+  int ret = sendCom();
+
+  res.success = 0;
+
+  if (ret)
+    return false;
+
+  res.success = 1;
+
+  return true;
+}
+
+void ODOM_DATA_PUBLISH(unsigned char *buf) {}
+
+void IMU_DATA_PUBLISH(unsigned char *buf) {
+
+  int16_t tmp;
+
+  memcpy(&tmp, buf, 2);
+  data.linear_acceleration.x = tmp * 9.8 / 16384.0;
+  memcpy(&tmp, buf + 2, 2);
+  data.linear_acceleration.y = tmp * 9.8 / 16384.0;
+  memcpy(&tmp, buf + 4, 2);
+  data.linear_acceleration.z = tmp * 9.8 / 16384.0;
+
+  memcpy(&tmp, buf + 8, 2);
+  data.angular_velocity.x = tmp * (M_PI / 180.0) / 131.0;
+  memcpy(&tmp, buf + 10, 2);
+  data.angular_velocity.y = tmp * (M_PI / 180.0) / 131.0;
+  memcpy(&tmp, buf + 12, 2);
+  data.angular_velocity.z = tmp * (M_PI / 180.0) / 131.0;
+
+  imu_pub.publish(data);
+}

qt_pi/qt_pi_serial/src/serial_driver.cpp

@@ -1,13 +1,47 @@
-#include <qt_pi/serial_driver.h>
+#include "qt_pi_serial/serial_driver.h"
+#include "stdio.h"
+
+unsigned char com_buf[buf_size];
+int com_buf_len;
+
+int sig = 1;
+
+pthread_t thread_id;
+int fd;
+
+unsigned char in_out_buf[buf_size];
+int payload_len;
+com_state state = IDLE;
+
+pthread_cond_t data_wait_cond;
+pthread_mutex_t data_wait_mutex;
+
+struct timespec ts;
+
+int open_port(const char *);
+void *serialDataIn(void *);
+
+int init_comm(const char *port) {
+
+  if ((fd = open_port(port)) < 0)
+    return -1;
+
+  pthread_create(&thread_id, NULL, serialDataIn, NULL);
+
+  pthread_mutex_init(&data_wait_mutex, NULL);
+  pthread_cond_init(&data_wait_cond, NULL);
+
+  return 0;
+}
 
 int open_port(const char *port) {
   int fd;
 
   fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
   if (fd == -1) {
-    perror("open_port: Unable to open - ");
+    return -1;
   } else
-    fcntl(fd, F_SETFL, FNDELAY);
+    fcntl(fd, F_SETFL, 0);
 
   struct termios options;
 
@@ -17,79 +51,59 @@ int open_port(const char *port) {
   cfsetospeed(&options, B115200);
 
   options.c_cflag |= (CLOCAL | CREAD);
-
   options.c_cflag &= ~PARENB;
   options.c_cflag &= ~CSIZE;
   options.c_cflag &= ~CSTOPB;
   options.c_cflag &= ~HUPCL;
   options.c_cflag |= CS8;
-  options.c_cc[VTIME] = 100;
+  options.c_cc[VTIME] = 5;
   options.c_cc[VMIN] = 1;
 
   if (tcsetattr(fd, TCSANOW, &options)) {
-    perror("Setting options failed.\n");
     close(fd);
+    return -1;
   }
 
   return (fd);
 }
 
-volatile int sig = 1;
-
 void runRecvCom(uint16_t com_len, unsigned char *com) {
 
   switch (com[COMB]) {
 
   case GET_BAUDRATE:
-
+  case GET_ENCODERS:
-    unsigned long baud;
-    memcpy(&baud, com + DATASTRB, 4);
-
-    BAUDRATE_CB(com[CIDB], baud);
-
-    break;
-
-  case GET_ENCODERS: {
-
-    long l, r;
-
-    memcpy(&l, com + DATASTRB, 4);
-    memcpy(&r, com + DATASTRB + 4, 4);
-
-    ENCODERS_CB(com[CIDB], l, r);
-
-    break;
-  }
-
   case GET_PID_P:
-
-    PID_P_CB(com[CIDB], com + DATASTRB);
-
-    break;
-
   case GET_BATTERY_LVL:
+  case EN_ODOM:
+  case EN_IMU:
+  case SET_PID_L:
+  case SET_PID_R:
 
-    uint16_t batt_lvl;
+    if (state != COM_SENT)
-    memcpy(&batt_lvl, com + DATASTRB, 2);
+      return;
 
-    BATT_LVL_CB(com[CIDB], batt_lvl);
+    pthread_mutex_lock(&data_wait_mutex);
+
+    com_buf_len = com_len;
+    memcpy(com_buf, com, com_len);
+    state = COM_RECEIVED;
+
+    pthread_cond_signal(&data_wait_cond);
+
+    pthread_mutex_unlock(&data_wait_mutex);
 
     break;
 
   case IMU_DATA:
 
-    IMU_DATA_CB(com[CIDB], com + DATASTRB);
+    IMU_DATA_PUBLISH(com + DATASTRB);
 
     break;
 
   case ODOM_DATA:
 
-    long l, r;
+    ODOM_DATA_PUBLISH(com + DATASTRB);
-
-    memcpy(&l, com + DATASTRB, 4);
-    memcpy(&r, com + DATASTRB + 4, 4);
-
-    ODOM_DATA_CB(com[CIDB], l, r);
 
     break;
   }
@@ -97,12 +111,6 @@ void runRecvCom(uint16_t com_len, unsigned char *com) {
 
 void *serialDataIn(void *vargp) {
 
-  int *fd = (int *)vargp;
-
-  const uint8_t magic_start1 = 0xEE;
-  const uint8_t magic_start2 = 0xAE;
-  const uint8_t magic_end = 0xFC;
-
   enum PACKET_STATE {
     WAITING_FOR_NEW_COM,
     FIRST_BYTE_VERIFIED,
@@ -122,10 +130,11 @@ void *serialDataIn(void *vargp) {
   uint16_t com_len;
 
   PACKET_STATE curState = WAITING_FOR_NEW_COM;
+  pthread_yield();
 
   while (sig) {
 
-    inL = read(*fd, &inChar, 1);
+    inL = read(fd, &inChar, 1);
 
     switch (curState) {
 
@@ -194,60 +203,55 @@ void *serialDataIn(void *vargp) {
     }
   }
 }
-}
 
-void hex2bin(const char *in, size_t len, unsigned char *out) {
+int sendCom() {
 
-  static const unsigned char TBL[] = {
+  if (state != IDLE)
-      0,  1,  2,  3,  4,  5,  6,  7,  8,  9,  58, 59, 60, 61,
+    return 2;
-      62, 63, 64, 10, 11, 12, 13, 14, 15, 71, 72, 73, 74, 75,
-      76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89,
-      90, 91, 92, 93, 94, 95, 96, 10, 11, 12, 13, 14, 15};
 
-  static const unsigned char *LOOKUP = TBL - 48;
+  int data_len = com_buf_len - com_preamble_len;
 
-  const char *end = in + len;
+  in_out_buf[MB1P] = magic_start1;
+  in_out_buf[MB2P] = magic_start2;
+  memcpy(in_out_buf + LB1P, &com_buf_len, len_len);
+  in_out_buf[CIDBP] = com_buf[CIDB];
+  in_out_buf[COMBP] = com_buf[COMB];
+  memcpy(in_out_buf + DATASTRBP, com_buf + DATASTRB, data_len);
+  in_out_buf[preamble_len + com_buf_len] = magic_end;
 
-  while (in < end)
+  pthread_mutex_lock(&data_wait_mutex);
-    *(out++) = LOOKUP[*(in++)] << 4 | LOOKUP[*(in++)];
-}
 
-int main(int argc, char **argv) {
+  state = COM_SENT;
-  int l = 0;
-  int fd = open_port(argv[1]);
-  if (fd < 0)
-    return 0;
-  usleep(2000000);
-  pthread_t thread_id;
-  pthread_create(&thread_id, NULL, tRead, (void *)&fd);
 
-  unsigned char buffer[25];
+  write(fd, in_out_buf, net_extra_len + com_buf_len);
-  unsigned char in_string[50];
 
-  while (sig) {
+  clock_gettime(CLOCK_REALTIME, &ts);
-    fgets(in_string, 50, stdin);
+  ts.tv_sec += 2;
-    if (in_string[0] == 'q') {
+
-      sig = 0;
+  while (state != COM_RECEIVED) {
-      break;
+    if (pthread_cond_timedwait(&data_wait_cond, &data_wait_mutex, &ts) ==
+        ETIMEDOUT) {
+
+      pthread_mutex_unlock(&data_wait_mutex);
+      return 1;
     }
-
-    l = strlen(in_string) / 2;
-
-    hex2bin(in_string, 2 * l, buffer);
-
-    int n = write(fd, buffer, l);
-    printf("%d written\n", n);
-    for (int i = 0; i < n; i++)
-      printf("%x ", buffer[i]);
-    printf("\n");
   }
 
-  pthread_join(thread_id, NULL);
+  memcpy(com_buf, in_out_buf + preamble_len, com_buf_len);
-  // int res = read(fd, buffer, 25);
 
-  //  if(res > 0)
+  pthread_mutex_unlock(&data_wait_mutex);
-  //    for(int i = 0; i < res; i++)
+
-  //      printf("%x ", buffer[i]);
+  state = IDLE;
-  // printf("\n");
+
+  return 0;
+}
+
+void close_comm() {
+  sig = 0;
+  pthread_yield();
+  pthread_cancel(thread_id);
+  pthread_join(thread_id, nullptr);
+  pthread_mutex_destroy(&data_wait_mutex);
+  pthread_cond_destroy(&data_wait_cond);
   close(fd);
 }

qt_pi/qt_pi_serial/tests/comm_test.cpp

@@ -0,0 +1,107 @@
+#include <fcntl.h>
+#include <pthread.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
+void hex2bin(const char *in, size_t len, unsigned char *out) {
+
+  static const unsigned char TBL[] = {
+      0,  1,  2,  3,  4,  5,  6,  7,  8,  9,  58, 59, 60, 61,
+      62, 63, 64, 10, 11, 12, 13, 14, 15, 71, 72, 73, 74, 75,
+      76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89,
+      90, 91, 92, 93, 94, 95, 96, 10, 11, 12, 13, 14, 15};
+
+  static const unsigned char *LOOKUP = TBL - 48;
+
+  const char *end = in + len;
+
+  while (in < end)
+    *(out++) = LOOKUP[*(in++)] << 4 | LOOKUP[*(in++)];
+}
+
+int open_port(const char *port) {
+  int fd;
+
+  fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
+  if (fd == -1) {
+    return -1;
+  } else
+    fcntl(fd, F_SETFL, 0);
+
+  struct termios options;
+
+  tcgetattr(fd, &options);
+
+  cfsetispeed(&options, B115200);
+  cfsetospeed(&options, B115200);
+
+  options.c_cflag |= (CLOCAL | CREAD);
+  options.c_cflag &= ~PARENB;
+  options.c_cflag &= ~CSIZE;
+  options.c_cflag &= ~CSTOPB;
+  options.c_cflag &= ~HUPCL;
+  options.c_cflag |= CS8;
+  options.c_cc[VTIME] = 5;
+  options.c_cc[VMIN] = 1;
+
+  if (tcsetattr(fd, TCSANOW, &options)) {
+    close(fd);
+    return -1;
+  }
+
+  return (fd);
+}
+
+pthread_t thread_id;
+int fd;
+
+void *serialDataIn(void *);
+
+void *serialDataIn(void *vargp) {
+
+  int inL = 0;
+
+  unsigned char com_recv[25];
+  unsigned char in_buf[25];
+  unsigned char inChar;
+  int in_buf_i = 0;
+  uint16_t com_len;
+
+  pthread_yield();
+
+  while (1) {
+    printf("Waiting\n");
+    inL = read(fd, &inChar, 1);
+    printf("%x\n", inChar);
+  }
+}
+
+int main(int argc, char **argv) {
+  int l = 0;
+  fd = open_port(argv[1]);
+  if (fd < 0)
+    return 0;
+  usleep(2000000);
+
+  pthread_create(&thread_id, NULL, serialDataIn, NULL);
+  unsigned char buffer[32];
+  char in_string[64];
+
+  while (1) {
+
+    fgets(in_string, 64, stdin);
+    if (in_string[0] == 'q') {
+      pthread_cancel(thread_id);
+      close(fd);
+      return 0;
+    }
+
+    l = strlen(in_string) / 2;
+    printf("%d\n", l);
+    hex2bin(in_string, 2 * l, buffer);
+
+    int n = write(fd, buffer, l);
+  }
+}

qt_pi/scripts/qt_pi/arduino_driver.py

@@ -1,290 +0,0 @@
-
-import thread
-from math import pi as PI, degrees, radians
-import os
-import time
-import sys, traceback
-from serial.serialutil import SerialException
-from serial import Serial
-
-class Arduino:
-
-    def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, motors_reversed=False):
-
-        self.PID_RATE = 30 # Do not change this!  It is a fixed property of the Arduino PID controller.
-        self.PID_INTERVAL = 1000 / 30
-
-        self.port = port
-        self.baudrate = baudrate
-        self.timeout = timeout
-        self.encoder_count = 0
-        self.writeTimeout = timeout
-        self.interCharTimeout = timeout / 30.
-        self.motors_reversed = motors_reversed
-        # Keep things thread safe
-        self.mutex = thread.allocate_lock()
-
-    def connect(self):
-        try:
-            print "Connecting to Arduino on port", self.port, "..."
-            self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
-            # The next line is necessary to give the firmware time to wake up.
-            time.sleep(1)
-            test = self.get_baud()
-            if test != self.baudrate:
-                time.sleep(1)
-                test = self.get_baud()
-                if test != self.baudrate:
-                    raise SerialException
-            print "Connected at", self.baudrate
-            print "Arduino is ready."
-
-        except SerialException:
-            print "Serial Exception:"
-            print sys.exc_info()
-            print "Traceback follows:"
-            traceback.print_exc(file=sys.stdout)
-            print "Cannot connect to Arduino!"
-            os._exit(1)
-
-    def open(self):
-        ''' Open the serial port.
-        '''
-        self.port.open()
-
-    def close(self):
-        ''' Close the serial port.
-        '''
-        self.port.close()
-
-    def send(self, cmd):
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.
-        '''
-        self.port.write(cmd + '\r')
-
-    def recv(self, timeout=0.5):
-        timeout = min(timeout, self.timeout)
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.  Note: we use read() instead of readline() since
-            readline() tends to return garbage characters from the Arduino
-        '''
-        c = ''
-        value = ''
-        attempts = 0
-        while c != '\r':
-            c = self.port.read(1)
-            value += c
-            attempts += 1
-            if attempts * self.interCharTimeout > timeout:
-                return None
-
-        value = value.strip('\r')
-
-        return value
-
-    def recv_ack(self):
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.
-        '''
-        ack = self.recv(self.timeout)
-        return ack == 'OK'
-
-    def recv_int(self):
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.
-        '''
-        value = self.recv(self.timeout)
-        try:
-            return int(value)
-        except:
-            return None
-
-
-    def execute(self, cmd):
-        ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
-        '''
-        self.mutex.acquire()
-
-        try:
-            self.port.flushInput()
-        except:
-            pass
-
-        ntries = 1
-        attempts = 0
-
-        try:
-            self.port.write(cmd + '\r')
-            value = self.recv(self.timeout)
-            while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
-                try:
-                    self.port.flushInput()
-                    self.port.write(cmd + '\r')
-                    value = self.recv(self.timeout)
-                except:
-                    print "Exception executing command: " + cmd
-                attempts += 1
-        except:
-            self.mutex.release()
-            print "Exception executing command: " + cmd
-            value = None
-
-        self.mutex.release()
-        return int(value)
-
-    def recv_array(self):
-        ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.
-        '''
-        try:
-            values = self.recv(self.timeout * 8).split()
-            return map(int, values)
-        except:
-            return []
-
-    def execute_array(self, cmd):
-        ''' Thread safe execution of "cmd" on the Arduino returning an array.
-        '''
-        self.mutex.acquire()
-        try:
-            self.port.flushInput()
-        except:
-            pass
-        ntries = 1
-        attempts = 0
-
-        try:
-            self.port.write(cmd + '\r')
-            values = self.recv_array()
-            while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
-                try:
-                    self.port.flushInput()
-                    self.port.write(cmd + '\r')
-                    values = self.recv_array()
-                except:
-                    print("Exception executing command: " + cmd)
-                attempts += 1
-        except:
-            self.mutex.release()
-            print "Exception executing command: " + cmd
-            raise SerialException
-            return []
-
-        try:
-            values = map(int, values)
-        except:
-            values = []
-        self.mutex.release()
-        return values
-
-    def execute_ack(self, cmd):
-        ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
-        '''
-        self.mutex.acquire()
-
-        try:
-            self.port.flushInput()
-        except:
-            pass
-
-        ntries = 1
-        attempts = 0
-
-        try:
-            self.port.write(cmd + '\r')
-            ack = self.recv(self.timeout)
-            while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
-                try:
-                    self.port.flushInput()
-                    self.port.write(cmd + '\r')
-                    ack = self.recv(self.timeout)
-                except:
-                    print "Exception executing command: " + cmd
-            attempts += 1
-        except:
-            self.mutex.release()
-            print "execute_ack exception when executing", cmd
-            print sys.exc_info()
-            return 0
-
-        self.mutex.release()
-        return ack == 'OK'
-
-    def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo):
-        ''' Set the PID parameters on the Arduino
-        '''
-        print "Updating PID parameters"
-        cmd = 'L ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo)
-        self.execute_ack(cmd)
-        cmd = 'R ' + str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo)
-        self.execute_ack(cmd)
-
-    def get_baud(self):
-        ''' Get the current baud rate on the serial port.
-        '''
-        try:
-            return int(self.execute('b'));
-        except:
-            return None
-
-    def get_encoder_counts(self):
-        values = self.execute_array('e')
-        if len(values) != 2:
-            print "Encoder count was not 2"
-            raise SerialException
-            return None
-        else:
-            if self.motors_reversed:
-                values[0], values[1] = -values[0], -values[1]
-            return values
-
-    def reset_encoders(self):
-        ''' Reset the encoder counts to 0
-        '''
-        return self.execute_ack('r')
-
-    def drive(self, right, left):
-        ''' Speeds are given in encoder ticks per PID interval
-        '''
-        if self.motors_reversed:
-            left, right = -left, -right
-        return self.execute_ack('m %d %d' %(right, left))
-
-    def drive_m_per_s(self, right, left):
-        ''' Set the motor speeds in meters per second.
-        '''
-        left_revs_per_second = float(left) / (self.wheel_diameter * PI)
-        right_revs_per_second = float(right) / (self.wheel_diameter * PI)
-
-        left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
-        right_ticks_per_loop  = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
-
-        self.drive(right_ticks_per_loop , left_ticks_per_loop )
-
-    def stop(self):
-        ''' Stop both motors.
-        '''
-        self.drive(0, 0)
-    def get_batt_lvl(self):
-        return self.execute('B')
-if __name__ == "__main__":
-    if os.name == "posix":
-        portName = "/dev/ttyACM0"
-    else:
-        portName = "COM43" # Windows style COM port.
-
-    baudRate = 57600
-
-    qt_pi_Arduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
-    qt_pi_Arduino.connect()
-
-    print "Sleeping for 1 second..."
-    time.sleep(1)
-
-    print "Connection test successful.",
-
-    qt_pi_Arduino.stop()
-    qt_pi_Arduino.close()
-
-    print "Shutting down Arduino."

qt_pi/scripts/qt_pi/base_controller.py

@@ -1,248 +0,0 @@
-#!/usr/bin/env python
-
-import roslib; roslib.load_manifest('qt_pi')
-import rospy
-import os
-
-from math import sin, cos, pi
-from geometry_msgs.msg import Quaternion, Twist, Pose
-from std_msgs.msg import Float32
-from nav_msgs.msg import Odometry
-from tf.broadcaster import TransformBroadcaster
- 
-""" Class to receive Twist commands and publish Odometry data """
-class BaseController:
-    def __init__(self, arduino, base_frame, camera_frame, name="base_controllers"):
-        self.arduino = arduino
-        self.name = name
-        self.base_frame = base_frame
-        self.camera_frame = camera_frame
-        self.rate = float(rospy.get_param("~base_controller_rate", 10))
-        self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
-        self.stopped = False
-                 
-        pid_params = dict()
-        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
-        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
-        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
-        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
-        pid_params['lKp'] = rospy.get_param("~lKp", 20)
-        pid_params['lKd'] = rospy.get_param("~lKd", 12)
-        pid_params['lKi'] = rospy.get_param("~lKi", 0)
-        pid_params['lKo'] = rospy.get_param("~lKo", 50)
-        pid_params['rKp'] = rospy.get_param("~rKp", 20)
-        pid_params['rKd'] = rospy.get_param("~rKd", 12)
-        pid_params['rKi'] = rospy.get_param("~rKi", 0)
-        pid_params['rKo'] = rospy.get_param("~rKo", 50)
-        
-		
-        self.accel_limit = rospy.get_param('~accel_limit', 0.1)
-        self.motors_reversed = rospy.get_param("~motors_reversed", False)
-        self.gear_reduction = rospy.get_param("~gear_reduction", 1.0)
-        # Set up PID parameters and check for missing values
-        self.setup_pid(pid_params)
-            
-        # How many encoder ticks are there per meter?
-        self.ticks_per_meter = self.encoder_resolution  / (self.wheel_diameter * pi)
-        
-        # What is the maximum acceleration we will tolerate when changing wheel speeds?
-        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
-                
-        # Track how often we get a bad encoder count (if any)
-        self.bad_encoder_count = 0
-                        
-        now = rospy.Time.now()    
-        self.then = now # time for determining dx/dy
-        self.t_delta = rospy.Duration(1.0 / self.rate)
-        self.t_next = now + self.t_delta
-
-        # Internal data        
-        self.enc_left = None            # encoder readings
-        self.enc_right = None
-        self.x = 0                      # position in xy plane
-        self.y = 0
-        self.th = 0                     # rotation in radians
-        self.v_left = 0
-        self.v_right = 0
-        self.v_des_left = 0             # cmd_vel setpoint
-        self.v_des_right = 0
-        self.last_cmd_vel = now
-        self.batt_lvl = 0.0
-        # Subscriptions
-        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
-        
-        # Clear any old odometry info
-        self.arduino.reset_encoders()
-        
-        # Set up the odometry broadcaster
-        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
-        self.batt_lvl_pub = rospy.Publisher('battery_level', Float32, queue_size=1)
-        self.odomBroadcaster = TransformBroadcaster()
-        self.cameraBroadcaster = TransformBroadcaster()
-        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
-        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
-        
-    def setup_pid(self, pid_params):
-        # Check to see if any PID parameters are missing
-        missing_params = False
-        for param in pid_params:
-            if pid_params[param] == "":
-                print("*** PID Parameter " + param + " is missing. ***")
-                missing_params = True
-        
-        if missing_params:
-            os._exit(1)
-            
-        self.wheel_diameter = pid_params['wheel_diameter']
-        self.wheel_track = pid_params['wheel_track']
-        self.encoder_resolution = pid_params['encoder_resolution']
-        
-        self.lKp = pid_params['lKp']
-        self.lKd = pid_params['lKd']
-        self.lKi = pid_params['lKi']
-        self.lKo = pid_params['lKo']
-        
-        self.rKp = pid_params['rKp']
-        self.rKd = pid_params['rKd']
-        self.rKi = pid_params['rKi']
-        self.rKo = pid_params['rKo']
-        
-        self.arduino.update_pid(self.lKp, self.lKd, self.lKi, self.lKo,self.rKp, self.rKd, self.rKi, self.rKo)
-
-    def poll(self):
-        now = rospy.Time.now()
-        if now > self.t_next:
-            # Read the encoders
-            try:
-                left_enc, right_enc = self.arduino.get_encoder_counts()
-            except:
-                self.bad_encoder_count += 1
-                rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
-                return
-                            
-            dt = now - self.then
-            self.then = now
-            dt = dt.to_sec()
-            
-            # Calculate odometry
-            if self.enc_left == None:
-                dright = 0
-                dleft = 0
-            else:
-                dright = (right_enc - self.enc_right) / self.ticks_per_meter
-                dleft = (left_enc - self.enc_left) / self.ticks_per_meter
-
-            self.enc_right = right_enc
-            self.enc_left = left_enc
-            
-            dxy_ave = (dright + dleft) / 2.0
-            dth = (dright - dleft) / self.wheel_track
-            vxy = dxy_ave / dt
-            vth = dth / dt
-                
-            if (dxy_ave != 0):
-                dx = cos(dth) * dxy_ave
-                dy = -sin(dth) * dxy_ave
-                self.x += (cos(self.th) * dx - sin(self.th) * dy)
-                self.y += (sin(self.th) * dx + cos(self.th) * dy)
-    
-            if (dth != 0):
-                self.th += dth 
-    
-            quaternion = Quaternion()
-            quaternion.x = 0.0 
-            quaternion.y = 0.0
-            quaternion.z = sin(self.th / 2.0)
-            quaternion.w = cos(self.th / 2.0)
-    
-            # Create the odometry transform frame broadcaster.
-            self.odomBroadcaster.sendTransform(
-                (self.x, self.y, 0), 
-                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
-                rospy.Time.now(),
-                self.base_frame,
-                "odom"
-                )
-    	    #self.cameraBroadcaster.sendTransform((self.x + 0.1, self.y - 0.05, 0.125),
-            self.cameraBroadcaster.sendTransform((0.1, -0.05, 0.125),
-                         (0,0,0,1),
-                         rospy.Time.now(),
-                         self.camera_frame,
-                         self.base_frame)
-		
-            odom = Odometry()
-            odom.header.frame_id = "odom"
-            odom.child_frame_id = self.base_frame
-            odom.header.stamp = now
-            odom.pose.pose.position.x = self.x
-            odom.pose.pose.position.y = self.y
-            odom.pose.pose.position.z = 0
-            odom.pose.pose.orientation = quaternion
-            odom.twist.twist.linear.x = vxy
-            odom.twist.twist.linear.y = 0
-            odom.twist.twist.angular.z = vth
-
-            self.odomPub.publish(odom)
-            
-            if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
-                self.v_des_left = 0
-                self.v_des_right = 0
-                
-            if self.v_left < self.v_des_left:
-                self.v_left += self.max_accel
-                if self.v_left > self.v_des_left:
-                    self.v_left = self.v_des_left
-            else:
-                self.v_left -= self.max_accel
-                if self.v_left < self.v_des_left:
-                    self.v_left = self.v_des_left
-            
-            if self.v_right < self.v_des_right:
-                self.v_right += self.max_accel
-                if self.v_right > self.v_des_right:
-                    self.v_right = self.v_des_right
-            else:
-                self.v_right -= self.max_accel
-                if self.v_right < self.v_des_right:
-                    self.v_right = self.v_des_right
-            
-            # Set motor speeds in encoder ticks per PID loop
-            if not self.stopped:
-                self.arduino.drive(self.v_left, self.v_right)
-                
-            self.t_next = now + self.t_delta
-            self.batt_lvl = self.arduino.get_batt_lvl() / 100.0
-
-            self.batt_lvl_pub.publish(self.batt_lvl)
-    def stop(self):
-        self.stopped = True
-        self.arduino.drive(0, 0)
-            
-    def cmdVelCallback(self, req):
-        # Handle velocity-based movement requests
-        self.last_cmd_vel = rospy.Time.now()
-        
-        x = req.linear.x         # m/s
-        th = req.angular.z       # rad/s
-
-        if x == 0:
-            # Turn in place
-            right = th * self.wheel_track  * self.gear_reduction / 2.0
-            left = -right
-        elif th == 0:
-            # Pure forward/backward motion
-            left = right = x
-        else:
-            # Rotation about a point in space
-            left = x - th * self.wheel_track  * self.gear_reduction / 2.0
-            right = x + th * self.wheel_track  * self.gear_reduction / 2.0
-            
-        self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
-        self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
-        
-
-        
-
-    
-
-    

qt_pi/setup.py

@@ -1,11 +0,0 @@
-#!/usr/bin/env python
-
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-d = generate_distutils_setup(
-    packages=['qt_pi'],
-    package_dir={'': 'scripts'},
-    )
-
-setup(**d)

qt_pi/src/libraries/Arduino/Arduino.ino

@@ -1,96 +0,0 @@
-#include "qt_pi.h"
-
-#define _TASK_SLEEP_ON_IDLE_RUN
-#define _TASK_INLINE
-#define _TASK_TIMEOUT
-
-#include <TaskScheduler.h>
-
-Task *batt_mon;
-Task *diff_control;
-Task *move_cmd_timeout;
-Task *odom_out;
-Task *imu_out;
-Task *ser_comm_timeout;
-
-Scheduler ts;
-
-void setup() {
-
-    init_comm();
-    
-    init_enc();
-    
-    initMotorController();
-
-    batt_mon = new Task(BATT_INTERVAL, TASK_FOREVER, &updateBattLevel, &ts, true, &initBatt, NULL);
-    diff_control = new Task(PID_INTERVAL, TASK_FOREVER, &updatePID, &ts, true, &resetPID, NULL);
-    move_cmd_timeout = new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, true, NULL, NULL);
-    odom_out = new Task(ODOM_INTERVAL, TASK_FOREVER, &sendOdom, &ts, false, NULL, NULL);
-    imu_out = new Task(IMU_INTERVAL, TASK_FOREVER, &sendIMU, &ts, false,  &initIMU, NULL);
-    ser_comm_timeout = new Task(AUTO_STOP_INTERVAL, 1, &serCommTimeout, &ts, true, NULL, NULL);
-
-}
-
-void loop() 
-{
-
-  ts.execute();
-  proc_serial();
-
-}
-
-void move_cmd_timeout_reset()
-{
-//  move_cmd_timeout->resetTimeout();  
-}
-
-void enable_odom(bool a)
-{
-  switch(a)
-  {
-    case 0:
-      switch(odom_out->isEnabled())
-      {
-        case 1:
-          odom_out->disable();
-          break;
-      }
-      break;
-      
-    case 1:
-      switch(odom_out->isEnabled())
-      {
-        case 0:
-          odom_out->enable();
-          break;
-      }
-      break;
-  }
-  
-}
-
-void enable_imu(bool a)
-{
-  switch(a)
-  {
-    case 0:
-      switch(imu_out->isEnabled())
-      {
-        case 1:
-          imu_out->disable();
-          break;
-      }
-      break;
-      
-    case 1:
-      switch(imu_out->isEnabled())
-      {
-        case 0:
-          imu_out->enable();
-          break;
-      }
-      break;
-  }
-  
-}

qt_pi/src/libraries/Arduino/comm.cpp

@@ -1,365 +0,0 @@
-#include "qt_pi.h"
-
-const uint8_t magic_start1 = 0xEE;
-const uint8_t magic_start2 = 0xAE;
-const uint8_t magic_end    = 0xFC;
-
-#define buf_size          32
-
-/*
-#define  WAITING_FOR_NEW_COM    0
-#define  FIRST_BYTE_VERIFIED    1
-#define  SECOND_BYTE_VERIFIED   2
-#define  LENGTH_INPUT           3
-#define  READING_PAYLOAD        4
-#define  READING_COMPLETE       5
-#define  COM_RECEIVED           6
-*/
-enum COMM_STATE {
-  WAITING_FOR_NEW_COM,
-  FIRST_BYTE_VERIFIED, 
-  SECOND_BYTE_VERIFIED, 
-  LENGTH_INPUT,
-  READING_PAYLOAD, 
-  READING_COMPLETE, 
-  COM_RECEIVED
-};
-
-int curState = WAITING_FOR_NEW_COM;
-
-int com_len;
-
-uint8_t in_buf[buf_size];
-int in_buf_i;
-
-uint8_t ser_write_buf[buf_size];
-uint8_t com_recv[buf_size];
-
-int init_comm(){
-   
-  ser_write_buf[MB1P] = magic_start1;
-  ser_write_buf[MB2P] = magic_start2;
-  
-  Serial.begin(BAUDRATE);
-  Serial.setTimeout(0);
-  
-  resetComm();
-  
-  curState = WAITING_FOR_NEW_COM;
-   
-}
-
-int sendCommand(int16_t len)
-{
-  if((len + net_extra_len) > buf_size)
-    return -1;
-
-  memcpy(ser_write_buf + start_magic_len, &len, len_len);
-  
-  ser_write_buf[preamble_len + len] = magic_end;
-  
-  return(Serial.write(ser_write_buf, len + net_extra_len));
-    
-}
-
-int sendData(int len, uint8_t *buf){
-
-  if((len + net_extra_len) > buf_size)
-    return -1;
-
-  memcpy(ser_write_buf + preamble_len, buf, len);
-
-  return sendCommand(len);
-
-}
-
-int runInpCommand() {
-
-  int data_len = com_len;
-  
-  switch(com_recv[COMB]) {
-    
-  case GET_BAUDRATE:
-
-    if(com_len != GET_BAUDRATE_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-
-    memcpy(ser_write_buf + DATASTRBP, &BAUDRATE, 4);
-
-    data_len = 4;
-
-    break;
-  
-  case GET_ENCODERS:
-  {
-    
-    if(com_len != GET_ENCODERS_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-    
-    long l = readEncoder(LEFT);
-    long r = readEncoder(RIGHT);
-  
-    memcpy(ser_write_buf + DATASTRBP, &l, 4);
-    memcpy(ser_write_buf + DATASTRBP + 4, &r, 4);
-  
-    data_len = 8;
-     
-    break;
-  }
-  
-  case RESET_ENCODERS:
-    
-    if(com_len != RESET_ENCODERS_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-
-    resetEncoders();
-    resetPID();
-    
-    data_len = 0;
-    
-    break;
-  
-  case SET_SPEEDS:
-
-    if(com_len != SET_SPEEDS_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-    
-    move_cmd_timeout_reset();
-    
-    if (com_recv[DATASTRB] == 0 && com_recv[DATASTRB + 1] == 0) {
-      
-      setMotorSpeeds(0, 0);
-      
-      resetPID();
-      
-      setMoving(false);
-      
-    }
-    else
-    {
-      setMoving(true);
-    }
-    
-    setTicksPerFrame(com_recv + DATASTRB);
-    
-    data_len = 0;
-    
-    break;
-  
-  case SET_PID_L:
-
-    if(com_len != SET_PID_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-  
-    setPID_L(com_recv + DATASTRB);
-    
-    data_len = 0;
-    
-    break;
-  
-  case SET_PID_R:
-
-    if(com_len != SET_PID_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-    
-    setPID_R(com_recv + DATASTRB);
-    
-    data_len = 0;
-    
-    break;
-  
-  case GET_PID_P:
-    
-    if(com_len != GET_PID_P_LEN)
-    {
-      data_len = -1;
-      break;
-    }  
-       
-    getPID(ser_write_buf + DATASTRBP);
-  
-    data_len = 8;
-    
-    break;
-  
-  case GET_BATTERY_LVL:  
-
-    if(com_len != GET_BATTERY_LVL_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-    
-    getBattLvl(ser_write_buf + DATASTRBP);
-    
-    data_len = 2;
-  
-    break;
-  
-  case EN_IMU:
-
-    if(com_len != EN_IMU_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-       
-    enable_imu(com_recv[DATASTRB]);
-    
-    ser_write_buf[DATASTRBP] = 1;
-    data_len = 1;
-
-    break;
-  
-  case EN_ODOM:
-    
-    if(com_len != EN_ODOM_LEN)
-    {
-      data_len = -1;
-      break;
-    }
-    
-    enable_odom(com_recv[DATASTRB]);   
-
-    ser_write_buf[DATASTRBP] = 1;
-    data_len = 1;
-    
-    break;
-
-  
-  default:
-    
-    ser_write_buf[DATASTRBP] = com_recv[COMB];
-    com_recv[COMB] = INVALID;
-    data_len = 1;
-    
-    break;
-  }
-
-  if(data_len == -1)
-  {
-    ser_write_buf[COMBP] = INVALID_LENGTH;
-    ser_write_buf[CIDBP] = com_recv[CIDB];
-    ser_write_buf[DATASTRBP] = com_recv[COMB];
-    data_len = 1;
-  }
-  else
-  {
-    ser_write_buf[COMBP] = com_recv[COMB];
-    ser_write_buf[CIDBP] = com_recv[CIDB];
-  }
-  
-  sendCommand(data_len + com_preamble_len);
-
-}
-
-void resetComm(){
-  com_len = 1;
-  in_buf_i = 0;
-}
-
-uint8_t temp_buf[64];
-void serCommTimeout(){
-  Serial.readBytes(temp_buf, 64);
-}
-
-void proc_serial(){
-  
-  if(!Serial.available())
-    return;
-
-  move_cmd_timeout_reset();
-  
-  switch(curState){
-    
-      case WAITING_FOR_NEW_COM:
-
-          if(Serial.read() == magic_start1)
-          {
-            curState = FIRST_BYTE_VERIFIED;
-          }
-          
-          break;
-          
-      case FIRST_BYTE_VERIFIED:
-
-          if(Serial.read() == magic_start2)
-          {
-            curState = SECOND_BYTE_VERIFIED;
-          }
-          else
-          {
-            curState = WAITING_FOR_NEW_COM;
-          }
-          
-          break;
-          
-      case SECOND_BYTE_VERIFIED:
-
-          com_len = Serial.read();
-          curState = LENGTH_INPUT;
-
-          break;
-          
-      case LENGTH_INPUT:
-
-          com_len = com_len | (Serial.read() << 8);
-
-          if(com_len < 20)
-          {
-            curState = READING_PAYLOAD;
-          }
-          else
-          {
-            curState = WAITING_FOR_NEW_COM;
-          }
-              
-          break;
-          
-      case READING_PAYLOAD:
-
-          in_buf_i += Serial.readBytes(&in_buf[in_buf_i], com_len - in_buf_i);
-
-          if(in_buf_i == com_len)
-          {
-            curState = READING_COMPLETE;
-          }
-          break;
-          
-      case READING_COMPLETE:
-
-          if(Serial.read() == magic_end){
-
-              memcpy(com_recv, in_buf, com_len);
-          
-              runInpCommand();
-
-              curState = WAITING_FOR_NEW_COM;
-          }
-          
-          resetComm();
-          
-          break;
-      }
-    
-    if((Serial.available() > 0) && curState != WAITING_FOR_NEW_COM)
-      proc_serial();
-  }

qt_pi/src/libraries/Arduino/commands.h

@@ -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

qt_pi/src/libraries/Arduino/payload_specs.h

@@ -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