caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

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.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-19 18:42:55 -07:00
parent a573f72c1c
commit 39a7d482de
11 changed files with 455 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
software/environment/rover/UDEV_rules/99-rover-usb-serial.rules
View File

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

151
software/firmware/chassis_pan_tilt/chassis_pan_tilt.ino
View File

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

70
software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py
View File

@@ -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
self.run_thread_flag = False

28
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py
View File

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

47
software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py
View File

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

164
software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py Executable file
View File

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

26
software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_tester.py Executable file
View File

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

2
software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py
View File

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

0
software/ros_packages/rover_control/src/tower_pan_tilt_control/pan_tilt_tester.py → software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_pan_tilt_tester.py
View File

3
software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch
View File

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

2
software/ros_packages/rover_main/launch/rover/control.launch
View File

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