Initial Commit

CMakeLists.txt

@@ -0,0 +1,21 @@
+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}
+)

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

config/costmap_common_params.yaml

@@ -0,0 +1,11 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+footprint: [[-15, -12], [15, -12], [15, 12], [-15, 12]]
+#robot_radius: ir_of_robot
+inflation_radius: 0.55
+
+observation_sources: laser_scan_kinect
+
+laser_scan_kinect: {sensor_frame: kinect_frame, data_type: LaserScan, topic: /LaserScan, marking: true, clearing: true}
+
+point_cloud_kinect: {sensor_frame: kinect_frame, data_type: PointCloud, topic: /PointCloud, marking: true, clearing: true}

config/global_costmap_params.yaml

@@ -0,0 +1,5 @@
+global_costmap:
+  global_frame: /map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  static_map: true

config/local_costmap_params.yaml

@@ -0,0 +1,10 @@
+local_costmap:
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 6.0
+  height: 6.0
+  resolution: 0.05

config/qt_pi_arduino_params.yaml

@@ -0,0 +1,30 @@
+
+port: /dev/ttyUSB0
+baud: 57600
+timeout: 0.1
+
+rate: 100
+
+use_base_controller: True
+base_controller_rate: 50
+
+# For a robot that uses base_footprint, change base_frame to base_footprint
+base_frame: base_link
+camera_frame: camera_depth_frame
+# === Robot drivetrain parameters
+wheel_diameter: 0.0703464848466
+wheel_track: 0.22
+encoder_resolution: 240 # from Pololu for 131:1 motors
+motors_reversed: False
+gear_reduction: 1.0
+# === PID parameters
+lKp: 50
+lKd: 10
+lKi: 0
+lKo: 20
+rKp: 50
+rKd: 10
+rKi: 0
+rKo: 20
+
+#accel_limit: 0.1

launch/arduino.launch

@@ -0,0 +1,6 @@
+<launch>
+   <node name="arduino" pkg="qt_pi" type="arduino_node.py" output="screen">
+      <rosparam file="$(find qt_pi)/config/qt_pi_arduino_params.yaml" command="load" />
+   </node>
+
+</launch>

launch/move_base.launch

@@ -0,0 +1,17 @@
+<launch>
+  <master auto="start"/>
+
+  <!-- Run the map server -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find qt_pi)/map/Untitled.pgm 0.05"/>
+
+  <!--- Run AMCL -->
+  <include file="$(find amcl)/examples/amcl_diff.launch" />
+
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <rosparam file="$(find qt_pi)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find qt_pi)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find qt_pi)/config/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find qt_pi)/config/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find qt_pi)/config/base_local_planner_params.yaml" command="load" />
+  </node>
+</launch>

launch/qt_pi.launch

@@ -0,0 +1,17 @@
+<launch>
+   <node name="arduino" pkg="qt_pi" type="arduino_node.py" output="screen">
+      <rosparam file="$(find qt_pi)/config/qt_pi_arduino_params.yaml" command="load" />
+   </node>
+
+   <node name="kinect2_bridge_1455208236617866858" pkg="kinect2_bridge" type="kinect2_bridge">
+   </node>
+
+   <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
+      <param name="image" value="kinect2/sd/image_depth_rect" />
+   </node>
+
+   <node name="gmapping" pkg="gmapping" type="slam_gmapping">
+      <param name="base_frame" value="base_link" type="string" />
+   </node>
+
+</launch>

map/F404.yaml

@@ -0,0 +1,7 @@
+image: F404.pgm
+resolution: 0.050000
+origin: [-2000,-2000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

map/Untitled.yaml

@@ -0,0 +1,7 @@
+image: F404.pgm
+resolution: 0.050000
+origin: [10,10, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

nodes/arduino_node.py

@@ -0,0 +1,87 @@
+#!/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)

package.xml

@@ -0,0 +1,17 @@
+<package format="2">
+  <name>qt_pi</name>
+  <version>0.0.0</version>
+  <description>The qt_pi package</description>
+  <maintainer email="rishabhkundu13@gmail.com">Rishabh Kundu</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>python-serial</exec_depend>
+  <export>
+  </export>
+</package>

scripts/qt_pi/arduino_driver.py

@@ -0,0 +1,291 @@
+
+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]
+            print values
+            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."

scripts/qt_pi/base_controller.py

@@ -0,0 +1,248 @@
+#!/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()
+            
+            self.batt_lvl_pub.publish(((self.batt_lvl) / 5) * 5 * 12.85 / 900)
+    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)
+        
+
+        
+
+    
+
+    

setup.py

@@ -0,0 +1,11 @@
+#!/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)

src/libraries/Arduino/Arduino.ino

@@ -0,0 +1,205 @@
+#define BAUDRATE     57600
+#define MAX_PWM        255
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+#include "commands.h"
+#include "motor_driver.h"
+#include "encoder_driver.h"
+#include "diff_controller.h"
+#define PID_RATE           30     // Hz
+const int PID_INTERVAL = 1000 / PID_RATE;
+unsigned long nextPID = PID_INTERVAL;
+#define AUTO_STOP_INTERVAL 2000
+long lastMotorCommand = AUTO_STOP_INTERVAL;
+int arg = 0;
+int index = 0;
+char chr;
+char cmd;
+char argv1[16];
+char argv2[16];
+long arg1;
+long arg2;
+
+void resetCommand() {
+  cmd = NULL;
+  memset(argv1, 0, sizeof(argv1));
+  memset(argv2, 0, sizeof(argv2));
+  arg1 = 0;
+  arg2 = 0;
+  arg = 0;
+  index = 0;
+}
+
+int runCommand() {
+  int i = 0;
+  char *p = argv1;
+  char *str;
+  int pid_args[4];
+  arg1 = atoi(argv1);
+  arg2 = atoi(argv2);
+  
+  switch(cmd) {
+  case GET_BAUDRATE:
+    Serial.println(BAUDRATE);
+    break;
+    
+  case READ_ENCODERS:
+    Serial.print(readEncoder(LEFT));
+    Serial.print(" ");
+    Serial.println(readEncoder(RIGHT));
+    break;
+
+   case RESET_ENCODERS:
+    resetEncoders();
+    resetPID();
+    Serial.println("OK");
+    break;
+
+  case MOTOR_SPEEDS:
+    /* Reset the auto stop timer */
+    lastMotorCommand = millis();
+    if (arg1 == 0 && arg2 == 0) {
+      setMotorSpeeds(0, 0);
+      resetPID();
+      moving = 0;
+    }
+    else moving = 1;
+    leftPID.TargetTicksPerFrame = arg1;
+    rightPID.TargetTicksPerFrame = arg2;
+    Serial.println("OK");
+    break;
+    
+  case UPDATE_PID_L:
+    while ((str = strtok_r(p, ":", &p)) != '\0') {
+       pid_args[i] = atoi(str);
+       i++;
+    }
+    leftPID.Kp = pid_args[0];
+    leftPID.Kd = pid_args[1];
+    leftPID.Ki = pid_args[2];
+    leftPID.Ko = pid_args[3];
+    Serial.println("OK");
+    break;
+
+  case UPDATE_PID_R:
+    while ((str = strtok_r(p, ":", &p)) != '\0') {
+       pid_args[i] = atoi(str);
+       i++;
+    }
+    rightPID.Kp = pid_args[0];
+    rightPID.Kd = pid_args[1];
+    rightPID.Ki = pid_args[2];
+    rightPID.Ko = pid_args[3];
+    Serial.println("OK");
+    break;
+
+  case DISP_PID_P:
+    Serial.println(leftPID.Kp);
+    Serial.println(leftPID.Kd);
+    Serial.println(leftPID.Ki);
+    Serial.println(leftPID.Ko);
+    Serial.println(rightPID.Kp);
+    Serial.println(rightPID.Kd);
+    Serial.println(rightPID.Ki);
+    Serial.println(rightPID.Ko);
+    Serial.println("OK");
+    break;
+  case BATTERY_LVL:
+	Serial.println(analogRead(A0));
+	break;
+  default:
+    Serial.println("Invalid Command");
+    break;
+  }
+}
+
+/* Setup function--runs once at startup. */
+void setup() {
+  Serial.begin(BAUDRATE);
+    pinMode(A0,INPUT);
+    digitalWrite(A0,1);
+    DDRC &= ~(1<<LEFT_ENC_PIN_A);
+    DDRC &= ~(1<<LEFT_ENC_PIN_B);
+    DDRD &= ~(1<<RIGHT_ENC_PIN_A);
+    DDRD &= ~(1<<RIGHT_ENC_PIN_B);
+    
+    //enable pull up resistors
+    PORTC |= (1<<LEFT_ENC_PIN_A);
+    PORTC |= (1<<LEFT_ENC_PIN_B);
+    PORTD |= (1<<RIGHT_ENC_PIN_A);
+    PORTD |= (1<<RIGHT_ENC_PIN_B);
+    
+    // tell pin change mask to listen to left encoder pins
+    PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
+    // tell pin change mask to listen to right encoder pins
+    PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
+    
+    // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
+    PCICR |= (1 << PCIE1) | (1 << PCIE2);
+    
+  initMotorController();
+  resetPID();
+  
+}
+
+
+void loop() {
+
+  
+  while (Serial.available() > 0) {
+    
+    // Read the next character
+    chr = Serial.read();
+
+    // Terminate a command with a CR
+    if (chr == 13) {
+      if (arg == 1) argv1[index] = NULL;
+      else if (arg == 2) argv2[index] = NULL;
+      runCommand();
+      resetCommand();
+    }
+    // Use spaces to delimit parts of the command
+    else if (chr == ' ') {
+      // Step through the arguments
+      if (arg == 0) arg = 1;
+      else if (arg == 1)  {
+        argv1[index] = NULL;
+        arg = 2;
+        index = 0;
+      }
+      continue;
+    }
+    else {
+      if (arg == 0) {
+        // The first arg is the single-letter command
+        cmd = chr;
+      }
+      else if (arg == 1) {
+        // Subsequent arguments can be more than one character
+        argv1[index] = chr;
+        index++;
+      }
+      else if (arg == 2) {
+        argv2[index] = chr;
+        index++;
+      }
+    }
+  }
+
+  if (millis() > nextPID) {
+    updatePID();
+    nextPID += PID_INTERVAL;
+  }
+  
+  // Check to see if we have exceeded the auto-stop interval
+  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
+    setMotorSpeeds(0, 0);
+    moving = 0;
+  }
+  
+}
+

src/libraries/Arduino/commands.h

@@ -0,0 +1,16 @@
+#ifndef COMMANDS_H
+#define COMMANDS_H
+
+#define GET_BAUDRATE   'b'
+#define READ_ENCODERS  'e'
+#define MOTOR_SPEEDS   'm'
+#define RESET_ENCODERS 'r'
+#define UPDATE_PID_L   'L'
+#define UPDATE_PID_R   'R'
+#define DISP_PID_P     'z'
+#define BATTERY_LVL    'B'
+#define LEFT            0
+#define RIGHT           1
+
+#endif
+

src/libraries/Arduino/diff_controller.h

@@ -0,0 +1,81 @@
+
+typedef struct {
+  double TargetTicksPerFrame;
+  long Encoder;
+  long PrevEnc;
+
+  int PrevInput;
+  
+  int ITerm;
+  int Kp = 500;
+  int Kd = 100;
+  int Ki = 0;
+  int Ko = 50;
+
+  long output;
+
+}
+SetPointInfo;
+
+SetPointInfo leftPID, rightPID;
+
+
+unsigned char moving = 0;
+
+void resetPID(){
+   leftPID.TargetTicksPerFrame = 0.0;
+   leftPID.Encoder = readEncoder(LEFT);
+   leftPID.PrevEnc = leftPID.Encoder;
+   leftPID.output = 0;
+   leftPID.PrevInput = 0;
+   leftPID.ITerm = 0;
+
+   rightPID.TargetTicksPerFrame = 0.0;
+   rightPID.Encoder = readEncoder(RIGHT);
+   rightPID.PrevEnc = rightPID.Encoder;
+   rightPID.output = 0;
+   rightPID.PrevInput = 0;
+   rightPID.ITerm = 0;
+}
+
+void doPID(SetPointInfo * p) {
+  long Perror;
+  long output;
+  int input;
+
+  input = p->Encoder - p->PrevEnc;
+  Perror = p->TargetTicksPerFrame - input;
+
+  output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
+  p->PrevEnc = p->Encoder;
+
+  output += p->output;
+
+  if (output >= MAX_PWM)
+    output = MAX_PWM;
+  else if (output <= -MAX_PWM)
+    output = -MAX_PWM;
+  else
+
+  p->ITerm += (p->Ki) * Perror;
+
+  p->output = output;
+  p->PrevInput = input;
+}
+
+void updatePID() {
+
+  leftPID.Encoder = readEncoder(LEFT);
+  rightPID.Encoder = readEncoder(RIGHT);
+  
+  if (!moving){
+    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
+    return;
+  }
+
+
+  doPID(&leftPID);
+  doPID(&rightPID);
+  setMotorSpeeds(leftPID.output, rightPID.output);
+}
+

src/libraries/Arduino/encoder_driver.h

@@ -0,0 +1,10 @@
+  #define LEFT_ENC_PIN_A PC4
+  #define LEFT_ENC_PIN_B PC5
+  
+  #define RIGHT_ENC_PIN_A PD2
+  #define RIGHT_ENC_PIN_B PD3
+   
+long readEncoder(int i);
+void resetEncoder(int i);
+void resetEncoders();
+

src/libraries/Arduino/encoder_driver.ino

@@ -0,0 +1,43 @@
+  volatile long left_enc_pos = 0L;
+  volatile long right_enc_pos = 0L;
+  static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
+    
+  ISR (PCINT1_vect){
+  	static uint8_t enc_last=0;
+        
+	enc_last <<=2;
+  enc_last |= (PINC & (3 << 4)) >> 4;
+  
+  	left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+  }
+  
+  ISR (PCINT2_vect){
+        static uint8_t enc_last=0;
+          	
+	enc_last <<=2;
+  enc_last |= (PIND & (3 << 2)) >> 2;
+  
+  	right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+  }
+  
+  long readEncoder(int i) {
+    if (i == LEFT) return left_enc_pos;
+    else return right_enc_pos;
+  }
+
+  void resetEncoder(int i) {
+    if (i == LEFT){
+      left_enc_pos=0L;
+      return;
+    } else { 
+      right_enc_pos=0L;
+      return;
+    }
+  }
+
+void resetEncoders() {
+  resetEncoder(LEFT);
+  resetEncoder(RIGHT);
+}
+
+

src/libraries/Arduino/motor_driver.h

@@ -0,0 +1,10 @@
+  #define RIGHT_MOTOR_BACKWARD 5
+  #define LEFT_MOTOR_BACKWARD  6
+  #define RIGHT_MOTOR_FORWARD  9
+  #define LEFT_MOTOR_FORWARD   10
+  #define RIGHT_MOTOR_ENABLE 12
+  #define LEFT_MOTOR_ENABLE 13
+
+void initMotorController();
+void setMotorSpeed(int i, int spd);
+void setMotorSpeeds(int leftSpeed, int rightSpeed);

src/libraries/Arduino/motor_driver.ino

@@ -0,0 +1,32 @@
+  void initMotorController() {
+    digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
+    digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
+  }
+  
+  void setMotorSpeed(int i, int spd) {
+    unsigned char reverse = 0;
+  
+    if (spd < 0)
+    {
+      spd = -spd;
+      reverse = 1;
+    }
+    if (spd > 255)
+      spd = 255;
+    
+    if (i == LEFT) { 
+      if(spd == 0)           { digitalWrite(LEFT_MOTOR_FORWARD, 1); digitalWrite(LEFT_MOTOR_BACKWARD, 1); }
+      else if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
+      else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
+    }
+    else if (i == RIGHT){
+      if(spd == 0)           { digitalWrite(RIGHT_MOTOR_FORWARD, 1); digitalWrite(RIGHT_MOTOR_BACKWARD, 1); }
+      else if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
+      else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
+    }
+  }
+  
+  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
+    setMotorSpeed(LEFT, leftSpeed);
+    setMotorSpeed(RIGHT, rightSpeed);
+  }