Pan/tilt now works for both tower and chassis. Intelligently controllable and centerable using joystick. Added some error handling for map stuff. New firmware for chassis node.

software/environment/rover/UDEV_rules/99-rover-usb-serial.rules

@@ -16,7 +16,7 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_1_0"
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyTowerAndPanTilt"
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyOdometry"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_1_3"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyChassisPanTilt"
 
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_2_0"
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_2_1"

software/firmware/chassis_pan_tilt/chassis_pan_tilt.ino

@@ -1,9 +1,154 @@
+////////// Includes //////////
+#include <ModbusRtu.h>
+#include <Servo.h>
+
+////////// Hardware / Data Enumerations //////////
+enum HARDWARE {
+  RS485_EN = 2,
+  RS485_RX = 7,
+  RS485_TX = 8,
+
+  SERVO_PAN = 5,
+  SERVO_TILT = 4,
+
+  LED_RED = 1,
+  LED_GREEN = 32,
+  LED_BLUE = 6,
+
+  LED_BLUE_EXTRA = 13
+};
+
+enum MODBUS_REGISTERS {
+  CENTER_ALL = 0,           // Input/Output
+  PAN_ADJUST_POSITIVE = 1,  // Input/Output
+  PAN_ADJUST_NEGATIVE = 2,  // Input/Output
+  TILT_ADJUST_POSITIVE = 3, // Input/Output
+  TILT_ADJUST_NEGATIVE = 4, // Input/Output
+};
+
+////////// Global Variables //////////
+const uint8_t node_id = 1;
+const uint8_t mobus_serial_port_number = 3;
+
+uint16_t modbus_data[] = {0, 0, 0, 0, 0};
+uint8_t num_modbus_registers = 0;
+
+int8_t poll_state = 0;
+bool communication_good = false;
+uint8_t message_count = 0;
+
+// Pan/tilt hard limits
+const int pan_min = 530;
+const int pan_center = 1590;
+const int pan_max = 2460;
+
+const int tilt_min = 620;
+const int tilt_center = 1670;
+const int tilt_max = 2380;
+
+// Pan/tilt positions
+int pan_position = pan_center;
+int tilt_position = tilt_center;
+
+////////// Class Instantiations //////////
+Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
+
+Servo pan_servo;
+Servo tilt_servo;
+
 void setup() {
-  // put your setup code here, to run once:
+//  Serial.begin(9600);
+//  while(!Serial);
+  setup_hardware();
+
+  num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
+
+  slave.begin(115200);
+  slave.setTimeOut(150);
 
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
+  poll_modbus();
-
+  set_leds();
+  set_pan_tilt_adjustments();
 }
+
+void setup_hardware() {
+  // Setup pins as inputs / outputs
+  pinMode(HARDWARE::RS485_EN, OUTPUT);
+
+  pinMode(HARDWARE::SERVO_PAN, OUTPUT);
+  pinMode(HARDWARE::SERVO_TILT, OUTPUT);
+
+  pan_servo.attach(HARDWARE::SERVO_PAN);
+  tilt_servo.attach(HARDWARE::SERVO_TILT);
+
+  pan_servo.writeMicroseconds(pan_center);
+  tilt_servo.writeMicroseconds(tilt_center);
+
+  pinMode(HARDWARE::LED_RED, OUTPUT);
+  pinMode(HARDWARE::LED_GREEN, OUTPUT);
+  pinMode(HARDWARE::LED_BLUE, OUTPUT);
+
+  pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
+
+  // Set default pin states
+  digitalWrite(HARDWARE::LED_RED, LOW);
+  digitalWrite(HARDWARE::LED_GREEN, HIGH);
+  digitalWrite(HARDWARE::LED_BLUE, HIGH);
+
+  digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
+}
+
+void poll_modbus() {
+  poll_state = slave.poll(modbus_data, num_modbus_registers);
+  communication_good = !slave.getTimeOutState();
+}
+
+void set_leds() {
+  if (poll_state > 4) {
+    message_count++;
+    if (message_count > 2) {
+      digitalWrite(HARDWARE::LED_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA));
+      message_count = 0;
+    }
+
+    digitalWrite(HARDWARE::LED_GREEN, LOW);
+    digitalWrite(HARDWARE::LED_RED, HIGH);
+  } else if (!communication_good) {
+    digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
+    digitalWrite(HARDWARE::LED_GREEN, HIGH);
+    digitalWrite(HARDWARE::LED_RED, LOW);
+  }
+}
+
+void set_pan_tilt_adjustments() {
+  if (communication_good) {
+    if (modbus_data[MODBUS_REGISTERS::CENTER_ALL]) {
+      pan_servo.writeMicroseconds(constrain(pan_position, pan_min, pan_max));
+      tilt_servo.writeMicroseconds(constrain(tilt_position, tilt_min, tilt_max));
+
+      pan_position = pan_center;
+      tilt_position = tilt_center;
+
+      modbus_data[MODBUS_REGISTERS::CENTER_ALL] = 0;
+    }
+
+    pan_position = constrain(pan_position + modbus_data[MODBUS_REGISTERS::PAN_ADJUST_POSITIVE] - modbus_data[MODBUS_REGISTERS::PAN_ADJUST_NEGATIVE], pan_min, pan_max);
+    tilt_position = constrain(tilt_position + modbus_data[MODBUS_REGISTERS::TILT_ADJUST_POSITIVE] - modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE], tilt_min, tilt_max);
+
+    pan_servo.writeMicroseconds(pan_position);
+    tilt_servo.writeMicroseconds(tilt_position);
+
+//    Serial.print(pan_position);
+//    Serial.print("\t");
+//    Serial.println(tilt_position);
+
+    modbus_data[MODBUS_REGISTERS::PAN_ADJUST_POSITIVE] = 0;
+    modbus_data[MODBUS_REGISTERS::PAN_ADJUST_NEGATIVE] = 0;
+    modbus_data[MODBUS_REGISTERS::TILT_ADJUST_POSITIVE] = 0;
+    modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE] = 0;
+  }
+}
+

software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py

@@ -16,7 +16,8 @@ from rover_control.msg import DriveCommandMessage, TowerPanTiltControlMessage
 GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro"
 
 DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive"
-DEFAULT_PAN_TILT_COMMAND_TOPIC = "/rover_control/pan_tilt/control"
+DEFAULT_TOWER_PAN_TILT_COMMAND_TOPIC = "/rover_control/tower/pan_tilt/control"
+DEFAULT_CHASSIS_PAN_TILT_COMMAND_TOPIC = "/rover_control/chassis/pan_tilt/control"
 
 DRIVE_COMMAND_HERTZ = 20
 
@@ -31,8 +32,11 @@ CAMERA_CHANGE_TIME = 0.2
 GUI_ELEMENT_CHANGE_TIME = 0.2
 CAMERA_TOGGLE_CHANGE_TIME = 0.35
 
-PAN_TILT_X_AXIS_SCALAR = 3
+TOWER_PAN_TILT_X_AXIS_SCALAR = 2
-PAN_TILT_Y_AXIS_SCALAR = 20
+TOWER_PAN_TILT_Y_AXIS_SCALAR = 15
+
+CHASSIS_PAN_TILT_X_AXIS_SCALAR = 15
+CHASSIS_PAN_TILT_Y_AXIS_SCALAR = 15
 
 
 #####################################
@@ -67,12 +71,12 @@ class LogitechJoystick(QtCore.QThread):
             "five_pressed": 0,
             "six_pressed": 0,
 
+            "seven_pressed": 0,
+            "eight_pressed": 0,
+            "nine_pressed": 0,
+            "ten_pressed": 0,
             "eleven_pressed": 0,
             "twelve_pressed": 0,
-            "thirteen_pressed": 0,
-            "fourteen_pressed": 0,
-            "fifteen_pressed": 0,
-            "sixteen_pressed": 0,
         }
 
         self.raw_mapping_to_class_mapping = {
@@ -91,12 +95,12 @@ class LogitechJoystick(QtCore.QThread):
             "BTN_TOP2": "five_pressed",
             "BTN_PINKIE": "six_pressed",
 
+            "BTN_BASE": "seven_pressed",
+            "BTN_BASE2": "eight_pressed",
+            "BTN_BASE3": "nine_pressed",
+            "BTN_BASE4": "ten_pressed",
             "BTN_BASE5": "eleven_pressed",
-            "BTN_BASE6": "twelve_pressed",
+            "BTN_BASE6": "twelve_pressed"
-            "BTN_BASE3": "thirteen_pressed",
-            "BTN_BASE4": "fourteen_pressed",
-            "BTN_BASE": "fifteen_pressed",
-            "BTN_BASE2": "sixteen_pressed",
         }
 
         self.ready = False
@@ -125,6 +129,7 @@ class LogitechJoystick(QtCore.QThread):
             events = self.gamepad.read()
 
             for event in events:
+                # print event.code
                 if event.code in self.raw_mapping_to_class_mapping:
                     self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
 
@@ -148,6 +153,7 @@ class JoystickControlSender(QtCore.QThread):
 
         # ########## Reference to class init variables ##########
         self.shared_objects = shared_objects
+        self.video_coordinator = self.shared_objects["threaded_classes"]["Video Coordinator"]
         self.right_screen = self.shared_objects["screens"]["right_screen"]
         self.speed_limit_progress_bar = self.right_screen.speed_limit_progress_bar  # type: QtWidgets.QProgressBar
         self.left_drive_progress_bar = self.right_screen.left_drive_progress_bar  # type: QtWidgets.QProgressBar
@@ -167,7 +173,12 @@ class JoystickControlSender(QtCore.QThread):
         # ########## Class Variables ##########
         # Publishers
         self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1)
-        self.tower_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_PAN_TILT_COMMAND_TOPIC, TowerPanTiltControlMessage, queue_size=1)
+        self.tower_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_COMMAND_TOPIC,
+                                                                TowerPanTiltControlMessage, queue_size=1)
+        self.chassis_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_CHASSIS_PAN_TILT_COMMAND_TOPIC,
+                                                                  TowerPanTiltControlMessage, queue_size=1)
+
+        self.current_pan_tilt_selection = "no_pan_tilt"
 
         self.last_hat_x_was_movement = False
         self.last_hat_y_was_movement = False
@@ -183,7 +194,6 @@ class JoystickControlSender(QtCore.QThread):
 
     def run(self):
         while self.run_thread_flag:
-
             start_time = time()
 
             self.check_and_set_pause_state()
@@ -198,6 +208,8 @@ class JoystickControlSender(QtCore.QThread):
         self.set_left_drive_output__signal.connect(self.left_drive_progress_bar.setValue)
         self.set_right_drive_output__signal.connect(self.right_drive_progress_bar.setValue)
 
+        self.video_coordinator.pan_tilt_selection_changed__signal.connect(self.on_pan_tilt_selection_changed__slot)
+
     def check_and_set_pause_state(self):
         thumb_pressed = self.joystick.controller_states["thumb_pressed"]
         if thumb_pressed and (time() - self.last_pause_state_time) > PAUSE_STATE_CHANGE_TIME:
@@ -249,15 +261,32 @@ class JoystickControlSender(QtCore.QThread):
             self.last_camera_toggle_time = time()
 
     def publish_pan_tilt_control_commands(self):
+        button_eight = self.joystick.controller_states["seven_pressed"]
         hat_x = self.joystick.controller_states["hat_x_axis"]
         hat_y = self.joystick.controller_states["hat_y_axis"]
 
-        pan_tilt_message = TowerPanTiltControlMessage()
+        if (hat_x == 0 and not self.last_hat_x_was_movement) and (
-        pan_tilt_message.relative_pan_adjustment = hat_x * PAN_TILT_X_AXIS_SCALAR
+                hat_y == 0 and not self.last_hat_y_was_movement) and not button_eight:
-        pan_tilt_message.relative_tilt_adjustment = -(hat_y * PAN_TILT_Y_AXIS_SCALAR)
+            return
 
+        self.last_hat_x_was_movement = True if hat_x != 0 else False
+        self.last_hat_y_was_movement = True if hat_y != 0 else False
+
+        pan_tilt_message = TowerPanTiltControlMessage()
+
+        if button_eight:
+            pan_tilt_message.should_center = 1
+
+        if self.current_pan_tilt_selection == "tower_pan_tilt":
+            pan_tilt_message.relative_pan_adjustment = hat_x * TOWER_PAN_TILT_X_AXIS_SCALAR
+            pan_tilt_message.relative_tilt_adjustment = -(hat_y * TOWER_PAN_TILT_Y_AXIS_SCALAR)
             self.tower_pan_tilt_command_publisher.publish(pan_tilt_message)
 
+        elif self.current_pan_tilt_selection == "chassis_pan_tilt":
+            pan_tilt_message.relative_pan_adjustment = hat_x * CHASSIS_PAN_TILT_X_AXIS_SCALAR
+            pan_tilt_message.relative_tilt_adjustment = -(hat_y * CHASSIS_PAN_TILT_Y_AXIS_SCALAR)
+            self.chassis_pan_tilt_command_publisher.publish(pan_tilt_message)
+
     def get_drive_message(self, throttle_axis):
         drive_message = DriveCommandMessage()
 
@@ -278,6 +307,9 @@ class JoystickControlSender(QtCore.QThread):
 
         return drive_message
 
+    def on_pan_tilt_selection_changed__slot(self, selection):
+        self.current_pan_tilt_selection = selection
+
     def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
         start_signal.connect(self.start)
         signals_and_slots_signal.connect(self.connect_signals_and_slots)

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py

@@ -5,6 +5,7 @@
 from PyQt5 import QtCore, QtWidgets, QtGui
 from PIL.ImageQt import ImageQt
 from PIL import Image
+import numpy
 
 import logging
 
@@ -145,10 +146,11 @@ class RoverMapCoordinator(QtCore.QThread):
 
     def update_overlay(self):
         if self.latitude and self.longitude:
+            if not numpy.isnan(self.latitude) and not numpy.isnan(self.longitude):
+
                 longitude = self.latitude
                 latitude = self.longitude
 
-            print self.longitude, "  :  ", self.latitude
                 navigation_list = self._get_table_elements(self.navigation_label)
                 # landmark_list = self._get_table_elements(self.landmark_label)
                 landmark_list = []

software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py

@@ -36,6 +36,8 @@ class RoverVideoCoordinator(QtCore.QThread):
 
     update_element_stylesheet__signal = QtCore.pyqtSignal()
 
+    pan_tilt_selection_changed__signal = QtCore.pyqtSignal(str)
+
     def __init__(self, shared_objects):
         super(RoverVideoCoordinator, self).__init__()
 
@@ -81,6 +83,9 @@ class RoverVideoCoordinator(QtCore.QThread):
         self.msleep(3000)
 
         # Setup cameras
+        self.main_nav_index = -1
+        self.chassis_index = -1
+
         self.__get_cameras()
         self.__setup_video_threads()
 
@@ -105,6 +110,8 @@ class RoverVideoCoordinator(QtCore.QThread):
     def run(self):
         self.logger.debug("Starting Video Coordinator Thread")
 
+        self.__broadcast_current_pan_tilt_selection()
+
         while self.run_thread_flag:
             self.__set_max_resolutions()
             self.__toggle_background_cameras_if_needed()
@@ -115,6 +122,22 @@ class RoverVideoCoordinator(QtCore.QThread):
 
         self.logger.debug("Stopping Video Coordinator Thread")
 
+    def __broadcast_current_pan_tilt_selection(self):
+        setting = None
+        if self.current_label_for_joystick_adjust == 0:  # primary
+            setting = self.primary_label_current_setting
+        elif self.current_label_for_joystick_adjust == 1:  # secondary
+            setting = self.secondary_label_current_setting
+        elif self.current_label_for_joystick_adjust == 2:  # tertiary
+            setting = self.tertiary_label_current_setting
+
+        if setting == self.main_nav_index:
+            self.pan_tilt_selection_changed__signal.emit("tower_pan_tilt")
+        elif setting == self.chassis_index:
+            self.pan_tilt_selection_changed__signal.emit("chassis_pan_tilt")
+        else:
+            self.pan_tilt_selection_changed__signal.emit("no_pan_tilt")
+
     def __set_max_resolutions(self):
         if self.set_max_resolutions_flag:
             self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].set_hard_max_resolution(PRIMARY_LABEL_MAX)
@@ -178,7 +201,23 @@ class RoverVideoCoordinator(QtCore.QThread):
             if camera in names:
                 names.remove(camera)
 
-        self.valid_cameras = list(names)
+        self.valid_cameras = []
+        current_count = 0
+
+        if "main_navigation" in names:
+            self.valid_cameras.append("main_navigation")
+            self.main_nav_index = current_count
+            current_count += 1
+
+        if "chassis" in names:
+            self.valid_cameras.append("chassis")
+            self.chassis_index = current_count
+
+        if "undercarriage" in names:
+            self.valid_cameras.append("undercarriage")
+
+        if "end_effector" in names:
+            self.valid_cameras.append("end_effector")
 
     def __setup_video_threads(self):
         for camera in self.valid_cameras:
@@ -275,6 +314,8 @@ class RoverVideoCoordinator(QtCore.QThread):
         else:
             self.current_label_for_joystick_adjust = new_selection
 
+        self.__broadcast_current_pan_tilt_selection()
+
         self.update_element_stylesheet__signal.emit()
 
     def on_camera_selection_for_current_gui_element_changed(self, direction):
@@ -285,6 +326,8 @@ class RoverVideoCoordinator(QtCore.QThread):
         elif self.current_label_for_joystick_adjust == 2:  # tertiary
             self.tertiary_label_current_setting = (self.tertiary_label_current_setting + direction) % len(self.valid_cameras)
 
+        self.__broadcast_current_pan_tilt_selection()
+
         self.set_max_resolutions_flag = True
         self.update_element_stylesheet__signal.emit()
 

software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py

@@ -0,0 +1,164 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+
+from time import time, sleep
+
+import serial.rs485
+import minimalmodbus
+
+from std_msgs.msg import UInt8, UInt16
+
+# Custom Imports
+from rover_control.msg import TowerPanTiltControlMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "chassis_pan_tilt_control"
+
+DEFAULT_PORT = "/dev/rover/ttyChassisPanTilt"
+DEFAULT_BAUD = 115200
+
+DEFAULT_INVERT = False
+
+DEFAULT_PAN_TILT_CONTROL_TOPIC = "chassis/pan_tilt/control"
+
+PAN_TILT_NODE_ID = 1
+
+COMMUNICATIONS_TIMEOUT = 0.01  # Seconds
+
+RX_DELAY = 0.01
+TX_DELAY = 0.01
+
+DEFAULT_HERTZ = 20
+
+PAN_TILT_MODBUS_REGISTERS = {
+    "CENTER_ALL": 0,
+
+    "PAN_ADJUST_POSITIVE": 1,
+    "PAN_ADJUST_NEGATIVE": 2,
+    "TILT_ADJUST_POSITIVE": 3,
+    "TILT_ADJUST_NEGATIVE": 4
+}
+
+PAN_TILT_CONTROL_DEFAULT_MESSAGE = [
+    0,  # No centering
+    0,  # No pan positive adjustment
+    0,  # No pan negative adjustment
+    0,  # No tilt positive adjustment
+    0  # No tilt negative adjustement
+]
+
+NODE_LAST_SEEN_TIMEOUT = 2  # seconds
+
+
+#####################################
+# DriveControl Class Definition
+#####################################
+class ChassisPanTiltControl(object):
+    def __init__(self):
+        rospy.init_node(NODE_NAME)
+
+        self.port = rospy.get_param("~port", DEFAULT_PORT)
+        self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
+
+        self.pan_tilt_node_id = rospy.get_param("~pan_tilt_node_id", PAN_TILT_NODE_ID)
+
+        self.pan_tilt_control_subscriber_topic = rospy.get_param("~pan_tilt_control_topic",
+                                                                 DEFAULT_PAN_TILT_CONTROL_TOPIC)
+
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        self.pan_tilt_node = None
+        self.tower_node = None
+
+        self.connect_to_pan_tilt_and_tower()
+
+        self.pan_tilt_control_subscriber = rospy.Subscriber(self.pan_tilt_control_subscriber_topic,
+                                                            TowerPanTiltControlMessage,
+                                                            self.pan_tilt_control_callback)
+
+        self.pan_tilt_control_message = None
+        self.new_pan_tilt_control_message = False
+
+        self.modbus_nodes_seen_time = time()
+
+        self.run()
+
+    def __setup_minimalmodbus_for_485(self):
+        self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.pan_tilt_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                          delay_before_rx=RX_DELAY,
+                                                                          delay_before_tx=TX_DELAY)
+
+    def run(self):
+        self.send_startup_centering_command()
+
+        while not rospy.is_shutdown():
+            start_time = time()
+
+            try:
+                self.send_pan_tilt_control_message()
+                self.modbus_nodes_seen_time = time()
+
+            except Exception, error:
+                pass
+                # print "Error occurred:", error
+
+            if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
+                print "Chassis pan/tilt not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting."
+                return  # Exit so respawn can take over
+
+            time_diff = time() - start_time
+
+            sleep(max(self.wait_time - time_diff, 0))
+
+    def connect_to_pan_tilt_and_tower(self):
+        self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.pan_tilt_node_id))
+        self.__setup_minimalmodbus_for_485()
+
+    def send_startup_centering_command(self):
+        try:
+            registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
+            registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = 1
+            self.pan_tilt_node.write_registers(0, registers)
+        except:
+            pass
+
+    def send_pan_tilt_control_message(self):
+        if self.new_pan_tilt_control_message:
+            pan_tilt_control_message = self.pan_tilt_control_message  # type: TowerPanTiltControlMessage
+
+            registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
+            registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = int(pan_tilt_control_message.should_center)
+
+            if pan_tilt_control_message.relative_pan_adjustment >= 0:
+                registers[
+                    PAN_TILT_MODBUS_REGISTERS["PAN_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_pan_adjustment
+            else:
+                registers[PAN_TILT_MODBUS_REGISTERS[
+                    "PAN_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_pan_adjustment
+
+            if pan_tilt_control_message.relative_tilt_adjustment >= 0:
+                registers[PAN_TILT_MODBUS_REGISTERS[
+                    "TILT_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_tilt_adjustment
+            else:
+                registers[PAN_TILT_MODBUS_REGISTERS[
+                    "TILT_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_tilt_adjustment
+
+            self.pan_tilt_node.write_registers(0, registers)
+
+            self.new_pan_tilt_control_message = False
+        else:
+            self.pan_tilt_node.write_registers(0, PAN_TILT_CONTROL_DEFAULT_MESSAGE)
+
+    def pan_tilt_control_callback(self, pan_tilt_control):
+        self.pan_tilt_control_message = pan_tilt_control
+        self.new_pan_tilt_control_message = True
+
+if __name__ == "__main__":
+    ChassisPanTiltControl()

software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_tester.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+import rospy
+import time
+
+from rover_control.msg import TowerPanTiltControlMessage
+
+DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "chassis/pan_tilt/control"
+
+rospy.init_node("chassis_pan_tilt_tester")
+
+publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC, TowerPanTiltControlMessage, queue_size=1)
+
+time.sleep(2)
+
+message = TowerPanTiltControlMessage()
+message.should_center = 1
+
+publisher.publish(message)
+
+time.sleep(1)
+
+message = TowerPanTiltControlMessage()
+message.relative_pan_adjustment = -100
+message.relative_tilt_adjustment = -500
+
+publisher.publish(message)

software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py

@@ -27,7 +27,7 @@ DEFAULT_INVERT = False
 
 DEFAULT_TOWER_LIGHT_CONTROL_TOPIC = "tower/light/control"
 DEFAULT_TOWER_CO2_STATUS_TOPIC = "tower/status/co2"
-DEFAULT_PAN_TILT_CONTROL_TOPIC = "pan_tilt/control"
+DEFAULT_PAN_TILT_CONTROL_TOPIC = "tower/pan_tilt/control"
 
 TOWER_NODE_ID = 1
 PAN_TILT_NODE_ID = 2

software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch

@@ -7,7 +7,8 @@
             <param name="destination_port" value="17100" />
             <rosparam param="topics">
                 [{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0},
-                {name: "/rover_control/pan_tilt/control", compress: true, rate: 20.0}]
+                {name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.0},
+                {name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0}]
             </rosparam>
         </node>
 

software/ros_packages/rover_main/launch/rover/control.launch

@@ -34,5 +34,7 @@
         <node name="drive_coordinator" pkg="rover_control" type="drive_coordinator.py" respawn="true" output="screen"/>
 
         <node name="tower_and_pan_tilt" pkg="rover_control" type="tower_and_pan_tilt_control.py" respawn="true" output="screen"/>
+
+        <node name="chassis_pan_tilt" pkg="rover_control" type="chassis_pan_tilt_control.py" respawn="true" output="screen"/>
     </group>
 </launch>