From 39a7d482deb3943f684959cdaf33ea64a6ff2122 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Thu, 19 Jul 2018 18:42:55 -0700 Subject: [PATCH] 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. --- .../UDEV_rules/99-rover-usb-serial.rules | 2 +- .../chassis_pan_tilt/chassis_pan_tilt.ino | 151 +++++++++++++++- .../InputSystems/JoystickControlSender.py | 70 ++++++-- .../MapSystems/RoverMapCoordinator.py | 28 +-- .../VideoSystems/RoverVideoCoordinator.py | 47 ++++- .../chassis_pan_tilt_control.py | 164 ++++++++++++++++++ .../chassis_pan_tilt_tester.py | 26 +++ .../tower_and_pan_tilt_control.py | 2 +- ...ilt_tester.py => tower_pan_tilt_tester.py} | 0 .../topic_transport_senders.launch | 3 +- .../rover_main/launch/rover/control.launch | 2 + 11 files changed, 455 insertions(+), 40 deletions(-) create mode 100755 software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py create mode 100755 software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_tester.py rename software/ros_packages/rover_control/src/tower_pan_tilt_control/{pan_tilt_tester.py => tower_pan_tilt_tester.py} (100%) diff --git a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules index 9645ba8..2cc27db 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/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" diff --git a/software/firmware/chassis_pan_tilt/chassis_pan_tilt.ino b/software/firmware/chassis_pan_tilt/chassis_pan_tilt.ino index 95c2b6e..97d6de1 100644 --- a/software/firmware/chassis_pan_tilt/chassis_pan_tilt.ino +++ b/software/firmware/chassis_pan_tilt/chassis_pan_tilt.ino @@ -1,9 +1,154 @@ +////////// Includes ////////// +#include +#include + +////////// 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; + } +} + diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py index fe4ae43..7070752 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py +++ b/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 -PAN_TILT_Y_AXIS_SCALAR = 20 +TOWER_PAN_TILT_X_AXIS_SCALAR = 2 +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_BASE3": "thirteen_pressed", - "BTN_BASE4": "fourteen_pressed", - "BTN_BASE": "fifteen_pressed", - "BTN_BASE2": "sixteen_pressed", + "BTN_BASE6": "twelve_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,14 +261,31 @@ 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() - pan_tilt_message.relative_pan_adjustment = hat_x * PAN_TILT_X_AXIS_SCALAR - pan_tilt_message.relative_tilt_adjustment = -(hat_y * PAN_TILT_Y_AXIS_SCALAR) + if (hat_x == 0 and not self.last_hat_x_was_movement) and ( + hat_y == 0 and not self.last_hat_y_was_movement) and not button_eight: + return - self.tower_pan_tilt_command_publisher.publish(pan_tilt_message) + 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) @@ -292,4 +324,4 @@ class JoystickControlSender(QtCore.QThread): self.right_drive_progress_bar.setStyleSheet("background-color: transparent;") def on_kill_threads_requested__slot(self): - self.run_thread_flag = False \ No newline at end of file + self.run_thread_flag = False diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py index 7c3573c..bd45b33 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/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,20 +146,21 @@ class RoverMapCoordinator(QtCore.QThread): def update_overlay(self): if self.latitude and self.longitude: - longitude = self.latitude - latitude = self.longitude + if not numpy.isnan(self.latitude) and not numpy.isnan(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 = [] - self.overlay_image = self.overlay_image_object.update_new_location( - latitude, - longitude, - 70, - navigation_list, - landmark_list) - # self.overlay_image.save("something.png") + longitude = self.latitude + latitude = self.longitude + + navigation_list = self._get_table_elements(self.navigation_label) + # landmark_list = self._get_table_elements(self.landmark_label) + landmark_list = [] + self.overlay_image = self.overlay_image_object.update_new_location( + latitude, + longitude, + 70, + navigation_list, + landmark_list) + # self.overlay_image.save("something.png") def gps_position_updated_callback(self, data): self.latitude = data.latitude diff --git a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py index 54d8f8c..6099b98 100644 --- a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py +++ b/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,16 +314,20 @@ 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): - if self.current_label_for_joystick_adjust == 0: # primary + if self.current_label_for_joystick_adjust == 0: # primary self.primary_label_current_setting = (self.primary_label_current_setting + direction) % len(self.valid_cameras) elif self.current_label_for_joystick_adjust == 1: # secondary self.secondary_label_current_setting = (self.secondary_label_current_setting + direction) % len(self.valid_cameras) 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() diff --git a/software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py b/software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py new file mode 100755 index 0000000..3d39a7c --- /dev/null +++ b/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() diff --git a/software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_tester.py b/software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_tester.py new file mode 100755 index 0000000..39be95a --- /dev/null +++ b/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) \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py index 292278e..112083d 100755 --- a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py +++ b/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 diff --git a/software/ros_packages/rover_control/src/tower_pan_tilt_control/pan_tilt_tester.py b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_pan_tilt_tester.py similarity index 100% rename from software/ros_packages/rover_control/src/tower_pan_tilt_control/pan_tilt_tester.py rename to software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_pan_tilt_tester.py diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch index 1f78383..57369c3 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch @@ -7,7 +7,8 @@ [{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}] diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index 8acd219..1a4ca14 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -34,5 +34,7 @@ + +