mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 18:21:15 +00:00
Merge remote-tracking branch 'upstream/master'
This commit is contained in:
Ken Steinfeldt
91
software/environment/rover/auto_poweroff/auto_poweroff.py
Executable file
91
software/environment/rover/auto_poweroff/auto_poweroff.py
Executable file
@@ -0,0 +1,91 @@
|
||||
#!/usr/bin/env python
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from time import time, sleep
|
||||
from os.path import exists, dirname, realpath
|
||||
from os import system, chdir
|
||||
import sys
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
UDEV_RULES_PATHS = ["../UDEV_rules/99-rover-usb-serial.rules"]
|
||||
|
||||
SHUTDOWN_TIMEOUT = 5
|
||||
|
||||
|
||||
#####################################
|
||||
# get_script_path Definition
|
||||
#####################################
|
||||
def get_script_path():
|
||||
return dirname(realpath(sys.argv[0]))
|
||||
|
||||
|
||||
#####################################
|
||||
# udev_parser Definition
|
||||
#####################################
|
||||
def udev_parser(rules_paths):
|
||||
device_paths = {}
|
||||
|
||||
script_path = get_script_path()
|
||||
chdir(script_path)
|
||||
|
||||
for current_file in rules_paths:
|
||||
lines = open(current_file).readlines()
|
||||
|
||||
for line in lines:
|
||||
if line[0] != "#" and line[0] != "\n":
|
||||
current_path = line.split("SYMLINK+=")[1].strip("\"\n")
|
||||
device_paths["/dev/" + current_path] = time()
|
||||
|
||||
return device_paths
|
||||
|
||||
|
||||
#####################################
|
||||
# AutoPoweroffWatchdog Class Definition
|
||||
#####################################
|
||||
class AutoPoweroffWatchdog(object):
|
||||
def __init__(self, devices, shutdown_timeout, do_poweroff=True):
|
||||
self.watched_devices = devices
|
||||
self.shutdown_timeout = shutdown_timeout
|
||||
self.do_poweroff = do_poweroff
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
self.run()
|
||||
|
||||
def run(self):
|
||||
while self.run_thread_flag:
|
||||
self.check_and_update_devices()
|
||||
self.initiate_shutdown_if_needed()
|
||||
sleep(0.25)
|
||||
|
||||
def check_and_update_devices(self):
|
||||
for device in self.watched_devices:
|
||||
if exists(device):
|
||||
self.watched_devices[device] = time()
|
||||
|
||||
def initiate_shutdown_if_needed(self):
|
||||
for device in self.watched_devices:
|
||||
if (time() - self.watched_devices[device]) < self.shutdown_timeout:
|
||||
return
|
||||
|
||||
if self.do_poweroff:
|
||||
system("sudo wall -n No devices seen for %s seconds. Powering down. Poweroff script exiting." %
|
||||
self.shutdown_timeout)
|
||||
system("sudo poweroff")
|
||||
exit()
|
||||
else:
|
||||
system("sudo wall -n No devices seen for %s seconds, but not powering down. Poweroff script exiting.")
|
||||
exit()
|
||||
|
||||
|
||||
#####################################
|
||||
# Main
|
||||
#####################################
|
||||
if __name__ == "__main__":
|
||||
watched_devices = udev_parser(UDEV_RULES_PATHS)
|
||||
AutoPoweroffWatchdog(watched_devices, SHUTDOWN_TIMEOUT, True)
|
||||
86
software/environment/rover/startup/screenrc
Normal file
86
software/environment/rover/startup/screenrc
Normal file
@@ -0,0 +1,86 @@
|
||||
# the following two lines give a two-line status, with the current window highlighted
|
||||
hardstatus alwayslastline
|
||||
hardstatus string '%{= kG}[%{G}%H%? %1`%?%{g}][%= %{= kw}%-w%{+b yk} %n*%t%?(%u)%? %{-}%+w %=%{g}][%{B}%m/%d %{W}%C%A%{g}]'
|
||||
|
||||
# huge scrollback buffer
|
||||
defscrollback 5000
|
||||
|
||||
# no welcome message
|
||||
startup_message off
|
||||
|
||||
# 256 colors
|
||||
attrcolor b ".I"
|
||||
termcapinfo xterm 'Co#256:AB=\E[48;5;%dm:AF=\E[38;5;%dm'
|
||||
defbce on
|
||||
|
||||
# mouse tracking allows to switch region focus by clicking
|
||||
mousetrack on
|
||||
|
||||
# default windows
|
||||
screen -t RoverMain 1 bash -c "/home/nvidia/Github/Rover_2017_2018/software/environment/rover/startup/startup.sh"
|
||||
screen -t Shell 2 bash
|
||||
select 1
|
||||
bind c screen 1 # window numbering starts at 1 not 0
|
||||
bind 0 select 10
|
||||
|
||||
# get rid of silly xoff stuff
|
||||
bind s split
|
||||
|
||||
# layouts
|
||||
layout autosave on
|
||||
layout new one
|
||||
select 1
|
||||
layout new two
|
||||
select 1
|
||||
split
|
||||
resize -v +8
|
||||
focus down
|
||||
select 4
|
||||
focus up
|
||||
layout new three
|
||||
select 1
|
||||
split
|
||||
resize -v +7
|
||||
focus down
|
||||
select 3
|
||||
split -v
|
||||
resize -h +10
|
||||
focus right
|
||||
select 4
|
||||
focus up
|
||||
|
||||
layout attach one
|
||||
layout select one
|
||||
|
||||
# navigating regions with Ctrl-arrows
|
||||
bindkey "^[[1;5D" focus left
|
||||
bindkey "^[[1;5C" focus right
|
||||
bindkey "^[[1;5A" focus up
|
||||
bindkey "^[[1;5B" focus down
|
||||
|
||||
# switch windows with F3 (prev) and F4 (next)
|
||||
bindkey "^[OR" prev
|
||||
bindkey "^[OS" next
|
||||
|
||||
# switch layouts with Ctrl+F3 (prev layout) and Ctrl+F4 (next)
|
||||
bindkey "^[O1;5R" layout prev
|
||||
bindkey "^[O1;5S" layout next
|
||||
|
||||
# F2 puts Screen into resize mode. Resize regions using hjkl keys.
|
||||
bindkey "^[OQ" eval "command -c rsz" # enter resize mode
|
||||
|
||||
# use hjkl keys to resize regions
|
||||
bind -c rsz h eval "resize -h -5" "command -c rsz"
|
||||
bind -c rsz j eval "resize -v -5" "command -c rsz"
|
||||
bind -c rsz k eval "resize -v +5" "command -c rsz"
|
||||
bind -c rsz l eval "resize -h +5" "command -c rsz"
|
||||
|
||||
# quickly switch between regions using tab and arrows
|
||||
bind -c rsz \t eval "focus" "command -c rsz" # Tab
|
||||
bind -c rsz -k kl eval "focus left" "command -c rsz" # Left
|
||||
bind -c rsz -k kr eval "focus right" "command -c rsz" # Right
|
||||
bind -c rsz -k ku eval "focus up" "command -c rsz" # Up
|
||||
bind -c rsz -k kd eval "focus down" "command -c rsz" # Down
|
||||
|
||||
# Clean up screen display with files
|
||||
altscreen on
|
||||
6
software/environment/rover/startup/startup.sh
Executable file
6
software/environment/rover/startup/startup.sh
Executable file
@@ -0,0 +1,6 @@
|
||||
#!/bin/bash
|
||||
source /opt/ros/kinetic/setup.bash
|
||||
source /home/nvidia/catkin_workspace/devel/setup.bash
|
||||
/home/nvidia/Github/Rover_2017_2018/software/environment/rover/auto_poweroff/auto_poweroff.py &
|
||||
roslaunch rover_main rover.launch
|
||||
exec bash
|
||||
@@ -11,6 +11,7 @@ folders_to_link=(
|
||||
rover_control
|
||||
nimbro_topic_transport
|
||||
rover_main
|
||||
rover_camera
|
||||
)
|
||||
|
||||
# Print heading
|
||||
|
||||
@@ -11,7 +11,7 @@ launch_dir_length_without_current_folder=$(($launch_dir_length-$current_folder_n
|
||||
script_launch_path="${launch_dir:0:$launch_dir_length_without_current_folder}/src"
|
||||
cd $script_launch_path
|
||||
|
||||
sleep 5
|
||||
sleep 1
|
||||
|
||||
export DISPLAY=:0
|
||||
python ground_station.py
|
||||
@@ -19,6 +19,11 @@ DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_dri
|
||||
|
||||
DRIVE_COMMAND_HERTZ = 15
|
||||
|
||||
Y_AXIS_DEADBAND = 0.05
|
||||
X_AXIS_DEADBAND = 0.05
|
||||
|
||||
THROTTLE_MIN = 0.05
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
@@ -37,59 +42,51 @@ class LogitechJoystick(QtCore.QThread):
|
||||
self.gamepad = None # type: GamePad
|
||||
|
||||
self.controller_states = {
|
||||
"left_stick_x_axis": 0,
|
||||
"x_axis": 512,
|
||||
"y_axis": 512,
|
||||
"left_stick_center_pressed": 0,
|
||||
|
||||
"right_stick_x_axis": 0,
|
||||
"right_stick_y_axis": 0,
|
||||
"right_stick_center_pressed": 0,
|
||||
|
||||
"left_trigger_z_axis": 0,
|
||||
"left_bumper_pressed": 0,
|
||||
|
||||
"z_axis": 128,
|
||||
"right_bumper_pressed": 0,
|
||||
"throttle_axis": 128,
|
||||
|
||||
"dpad_x": 0,
|
||||
"dpad_y": 0,
|
||||
"hat_x_axis": 0,
|
||||
"hat_y_axis": 0,
|
||||
|
||||
"select_pressed": 0,
|
||||
"start_pressed": 0,
|
||||
"home_pressed": 0,
|
||||
"trigger_pressed": 0,
|
||||
"thumb_pressed": 0,
|
||||
"three_pressed": 0,
|
||||
"four_pressed": 0,
|
||||
"five_pressed": 0,
|
||||
"six_pressed": 0,
|
||||
|
||||
"a_pressed": 0,
|
||||
"b_pressed": 0,
|
||||
"x_pressed": 0,
|
||||
"y_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 = {
|
||||
"ABS_X": "left_stick_x_axis",
|
||||
"ABS_X": "x_axis",
|
||||
"ABS_Y": "y_axis",
|
||||
"BTN_THUMBL": "left_stick_center_pressed",
|
||||
|
||||
"ABS_RX": "right_stick_x_axis",
|
||||
"ABS_RY": "right_stick_y_axis",
|
||||
"BTN_THUMBR": "right_stick_center_pressed",
|
||||
|
||||
"ABS_Z": "left_trigger_z_axis",
|
||||
"BTN_TL": "left_bumper_pressed",
|
||||
|
||||
"ABS_RZ": "z_axis",
|
||||
"BTN_TR": "right_bumper_pressed",
|
||||
"ABS_THROTTLE": "throttle_axis",
|
||||
|
||||
"ABS_HAT0X": "dpad_x",
|
||||
"ABS_HAT0Y": "dpad_y",
|
||||
"ABS_HAT0X": "hat_x_axis",
|
||||
"ABS_HAT0Y": "hat_y_axis",
|
||||
|
||||
"BTN_SELECT": "select_pressed",
|
||||
"BTN_START": "start_pressed",
|
||||
"BTN_MODE": "home_pressed",
|
||||
"BTN_TRIGGER": "trigger_pressed",
|
||||
"BTN_THUMB": "thumb_pressed",
|
||||
"BTN_THUMB2": "three_pressed",
|
||||
"BTN_TOP": "four_pressed",
|
||||
"BTN_TOP2": "five_pressed",
|
||||
"BTN_PINKIE": "six_pressed",
|
||||
|
||||
"BTN_SOUTH": "a_pressed",
|
||||
"BTN_EAST": "b_pressed",
|
||||
"BTN_NORTH": "x_pressed",
|
||||
"BTN_WEST": "y_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",
|
||||
}
|
||||
|
||||
self.ready = False
|
||||
@@ -107,7 +104,6 @@ class LogitechJoystick(QtCore.QThread):
|
||||
|
||||
def __setup_controller(self):
|
||||
for device in devices.gamepads:
|
||||
print device
|
||||
if device.name == GAME_CONTROLLER_NAME:
|
||||
self.gamepad = device
|
||||
|
||||
@@ -121,23 +117,27 @@ class LogitechJoystick(QtCore.QThread):
|
||||
for event in events:
|
||||
if event.code in self.raw_mapping_to_class_mapping:
|
||||
self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
|
||||
|
||||
self.ready = True
|
||||
# print "Logitech: %s" % self.controller_states
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
#####################################
|
||||
class RoverDriveSender(QtCore.QThread):
|
||||
set_speed_limit__signal = QtCore.pyqtSignal(int)
|
||||
set_left_drive_output__signal = QtCore.pyqtSignal(int)
|
||||
set_right_drive_output__signal = QtCore.pyqtSignal(int)
|
||||
|
||||
def __init__(self, shared_objects):
|
||||
super(RoverDriveSender, self).__init__()
|
||||
|
||||
# ########## Reference to class init variables ##########
|
||||
self.shared_objects = shared_objects
|
||||
self.right_screen = self.shared_objects["screens"]["right_screen"]
|
||||
self.primary_video_display_label = self.right_screen.primary_video_label # type:QtWidgets.QLabel
|
||||
self.secondary_video_display_label = self.right_screen.secondary_video_label # type:QtWidgets.QLabel
|
||||
self.tertiary_video_display_label = self.right_screen.tertiary_video_label # type:QtWidgets.QLabel
|
||||
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
|
||||
self.right_drive_progress_bar = self.right_screen.right_drive_progress_bar # type: QtWidgets.QProgressBar
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
@@ -154,34 +154,82 @@ class RoverDriveSender(QtCore.QThread):
|
||||
# Publishers
|
||||
self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1)
|
||||
|
||||
self.wait_time = 1 / DRIVE_COMMAND_HERTZ
|
||||
self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
|
||||
|
||||
self.drive_paused = True
|
||||
|
||||
def run(self):
|
||||
while self.run_thread_flag:
|
||||
|
||||
start_time = time()
|
||||
|
||||
self.check_and_set_pause_state()
|
||||
self.__update_and_publish()
|
||||
|
||||
time_diff = time() - start_time
|
||||
|
||||
self.msleep(max(self.wait_time - time_diff, 0))
|
||||
self.msleep(max(int(self.wait_time - time_diff), 0))
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
self.set_speed_limit__signal.connect(self.speed_limit_progress_bar.setValue)
|
||||
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)
|
||||
|
||||
def check_and_set_pause_state(self):
|
||||
if self.joystick.controller_states["thumb_pressed"]:
|
||||
self.drive_paused = not self.drive_paused
|
||||
self.msleep(350)
|
||||
self.show_changed_pause_state()
|
||||
|
||||
def __update_and_publish(self):
|
||||
throttle_axis = max((255 - self.joystick.controller_states["throttle_axis"]) / 255.0, THROTTLE_MIN)
|
||||
|
||||
if self.drive_paused:
|
||||
drive_message = DriveCommandMessage()
|
||||
else:
|
||||
drive_message = self.get_drive_message(throttle_axis)
|
||||
|
||||
left_output = abs(drive_message.drive_twist.linear.x - drive_message.drive_twist.angular.z)
|
||||
right_output = abs(drive_message.drive_twist.linear.x + drive_message.drive_twist.angular.z)
|
||||
|
||||
self.set_speed_limit__signal.emit(throttle_axis * 100)
|
||||
self.set_left_drive_output__signal.emit(left_output * 100)
|
||||
self.set_right_drive_output__signal.emit(right_output * 100)
|
||||
|
||||
self.drive_command_publisher.publish(drive_message)
|
||||
|
||||
def get_drive_message(self, throttle_axis):
|
||||
drive_message = DriveCommandMessage()
|
||||
|
||||
drive_message.drive_twist.linear.x = -(self.joystick.controller_states["y_axis"] - 512) / 1024.0
|
||||
drive_message.drive_twist.angular.z = -(self.joystick.controller_states["z_axis"] - 128) / 255.0
|
||||
self.drive_command_publisher.publish(drive_message)
|
||||
# print self.joystick.controller_states["y_axis"], self.joystick.controller_states["z_axis"]
|
||||
y_axis = throttle_axis * (-(self.joystick.controller_states["y_axis"] - 512) / 512.0)
|
||||
z_axis = throttle_axis * (-(self.joystick.controller_states["z_axis"] - 128) / 128.0)
|
||||
x_axis = throttle_axis * (-(self.joystick.controller_states["x_axis"] - 512) / 512.0)
|
||||
|
||||
if abs(y_axis) < Y_AXIS_DEADBAND:
|
||||
y_axis = 0
|
||||
|
||||
if abs(x_axis) < X_AXIS_DEADBAND and y_axis == 0:
|
||||
twist = z_axis
|
||||
else:
|
||||
twist = x_axis if y_axis >= 0 else -x_axis
|
||||
|
||||
drive_message.drive_twist.linear.x = y_axis
|
||||
drive_message.drive_twist.angular.z = twist
|
||||
|
||||
return drive_message
|
||||
|
||||
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)
|
||||
kill_signal.connect(self.on_kill_threads_requested__slot)
|
||||
|
||||
def show_changed_pause_state(self):
|
||||
if self.drive_paused:
|
||||
self.left_drive_progress_bar.setStyleSheet("background-color:darkred;")
|
||||
self.right_drive_progress_bar.setStyleSheet("background-color:darkred;")
|
||||
else:
|
||||
self.left_drive_progress_bar.setStyleSheet("background-color: transparent;")
|
||||
self.right_drive_progress_bar.setStyleSheet("background-color: transparent;")
|
||||
|
||||
def on_kill_threads_requested__slot(self):
|
||||
self.run_thread_flag = False
|
||||
|
||||
@@ -76,13 +76,13 @@ class RoverMapCoordinator(QtCore.QThread):
|
||||
RoverMap.OverlayImage(44.567161, -123.278432,
|
||||
self.google_maps_object.northwest,
|
||||
self.google_maps_object.southeast,
|
||||
self.google_maps_object.big_image[0],
|
||||
self.google_maps_object.big_image[1],
|
||||
self.google_maps_object.big_image.size[0],
|
||||
self.google_maps_object.big_image.size[1],
|
||||
1280, 720))
|
||||
|
||||
def _get_map_image(self):
|
||||
self.map_image = self.google_maps_object.display_image
|
||||
self.map_image.paste(self.overlay_image_object.display_image)
|
||||
# self.map_image.paste(self.overlay_image_object.display_image)
|
||||
# get overlay here
|
||||
qim = ImageQt(self.map_image)
|
||||
self.map_pixmap = QtGui.QPixmap.fromImage(qim)
|
||||
|
||||
@@ -9,20 +9,17 @@ import rospy
|
||||
|
||||
# Custom Imports
|
||||
import RoverVideoReceiver
|
||||
from rover_camera.msg import CameraControlMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
CAMERA_TOPIC_PATH = "/cameras/"
|
||||
EXCLUDED_CAMERAS = ["zed"]
|
||||
#
|
||||
PRIMARY_LABEL_MAX = (640, 360)
|
||||
SECONDARY_LABEL_MAX = (256, 144)
|
||||
TERTIARY_LABEL_MAX = (256, 144)
|
||||
|
||||
# PRIMARY_LABEL_MAX = (1280, 720)
|
||||
# SECONDARY_LABEL_MAX = (640, 360)
|
||||
# TERTIARY_LABEL_MAX = (640, 360)
|
||||
PRIMARY_LABEL_MAX = (640, 360)
|
||||
SECONDARY_LABEL_MAX = (640, 360)
|
||||
TERTIARY_LABEL_MAX = (640, 360)
|
||||
|
||||
|
||||
#####################################
|
||||
@@ -58,26 +55,35 @@ class RoverVideoCoordinator(QtCore.QThread):
|
||||
self.valid_cameras = []
|
||||
self.disabled_cameras = []
|
||||
|
||||
reset_camera_message = CameraControlMessage()
|
||||
reset_camera_message.enable_small_broadcast = True
|
||||
# Reset default cameras
|
||||
rospy.Publisher("/cameras/chassis/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message)
|
||||
rospy.Publisher("/cameras/undercarriage/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message)
|
||||
rospy.Publisher("/cameras/main_navigation/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message)
|
||||
rospy.Publisher("/cameras/end_effector/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message)
|
||||
|
||||
self.msleep(3000)
|
||||
|
||||
# Setup cameras
|
||||
self.__get_cameras()
|
||||
self.__setup_video_threads()
|
||||
|
||||
self.primary_label_current_setting = 0
|
||||
self.secondary_label_current_setting = 0
|
||||
self.tertiary_label_current_setting = 0
|
||||
self.secondary_label_current_setting = min(self.primary_label_current_setting + 1, len(self.valid_cameras))
|
||||
self.tertiary_label_current_setting = min(self.secondary_label_current_setting + 1, len(self.valid_cameras))
|
||||
|
||||
self.primary_label_max_resolution = -1
|
||||
self.secondary_label_max_resolution = -1
|
||||
self.tertiary_label_max_resolution = -1
|
||||
|
||||
self.set_max_resolutions_flag = True
|
||||
|
||||
self.first_image_received = False
|
||||
|
||||
def run(self):
|
||||
self.logger.debug("Starting Video Coordinator Thread")
|
||||
|
||||
self.__set_max_resolutions() # Do this initially so we don't try to disable cameras before they're set up
|
||||
self.msleep(500)
|
||||
|
||||
while self.run_thread_flag:
|
||||
self.__set_max_resolutions()
|
||||
self.__toggle_background_cameras_if_needed()
|
||||
@@ -88,37 +94,26 @@ class RoverVideoCoordinator(QtCore.QThread):
|
||||
self.logger.debug("Stopping Video Coordinator Thread")
|
||||
|
||||
def __set_max_resolutions(self):
|
||||
self.primary_label_max_resolution = self.camera_threads[
|
||||
self.valid_cameras[self.primary_label_current_setting]].current_max_resolution
|
||||
self.secondary_label_max_resolution = self.camera_threads[
|
||||
self.valid_cameras[self.secondary_label_current_setting]].current_max_resolution
|
||||
self.tertiary_label_max_resolution = self.camera_threads[
|
||||
self.valid_cameras[self.tertiary_label_current_setting]].current_max_resolution
|
||||
if self.set_max_resolutions_flag:
|
||||
self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].set_hard_max_resolution(PRIMARY_LABEL_MAX)
|
||||
|
||||
if self.primary_label_max_resolution != PRIMARY_LABEL_MAX:
|
||||
self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].change_max_resolution_setting(
|
||||
PRIMARY_LABEL_MAX)
|
||||
if self.secondary_label_current_setting != self.primary_label_current_setting:
|
||||
self.camera_threads[self.valid_cameras[self.secondary_label_current_setting]].set_hard_max_resolution(SECONDARY_LABEL_MAX)
|
||||
|
||||
if self.secondary_label_max_resolution != SECONDARY_LABEL_MAX and self.secondary_label_current_setting != self.primary_label_current_setting:
|
||||
self.camera_threads[self.valid_cameras[self.secondary_label_current_setting]].change_max_resolution_setting(
|
||||
SECONDARY_LABEL_MAX)
|
||||
if self.tertiary_label_current_setting != self.primary_label_current_setting and self.tertiary_label_current_setting != self.secondary_label_current_setting:
|
||||
self.camera_threads[self.valid_cameras[self.tertiary_label_current_setting]].set_hard_max_resolution(SECONDARY_LABEL_MAX)
|
||||
|
||||
if self.tertiary_label_max_resolution != TERTIARY_LABEL_MAX and self.tertiary_label_current_setting != self.primary_label_current_setting:
|
||||
self.camera_threads[self.valid_cameras[self.tertiary_label_current_setting]].change_max_resolution_setting(
|
||||
TERTIARY_LABEL_MAX)
|
||||
self.set_max_resolutions_flag = False
|
||||
|
||||
def __toggle_background_cameras_if_needed(self):
|
||||
if self.first_image_received:
|
||||
enabled = list({self.primary_label_current_setting, self.secondary_label_current_setting,
|
||||
self.tertiary_label_current_setting})
|
||||
enabled = list({self.primary_label_current_setting, self.secondary_label_current_setting,
|
||||
self.tertiary_label_current_setting})
|
||||
|
||||
for camera_index, camera_name in enumerate(self.valid_cameras):
|
||||
if camera_index not in enabled and camera_index not in self.disabled_cameras:
|
||||
self.camera_threads[camera_name].toggle_video_display()
|
||||
self.disabled_cameras.append(camera_index)
|
||||
elif camera_index in enabled and camera_index in self.disabled_cameras:
|
||||
self.camera_threads[camera_name].toggle_video_display()
|
||||
self.disabled_cameras.remove(camera_index)
|
||||
for camera_index, camera_name in enumerate(self.valid_cameras):
|
||||
if camera_index not in enabled and camera_index not in self.disabled_cameras and self.camera_threads[camera_name].video_enabled:
|
||||
self.camera_threads[camera_name].toggle_video_display()
|
||||
elif camera_index in enabled and camera_index not in self.disabled_cameras and not self.camera_threads[camera_name].video_enabled:
|
||||
self.camera_threads[camera_name].toggle_video_display()
|
||||
|
||||
def __get_cameras(self):
|
||||
topics = rospy.get_published_topics(CAMERA_TOPIC_PATH)
|
||||
@@ -167,33 +162,54 @@ class RoverVideoCoordinator(QtCore.QThread):
|
||||
def __change_display_source_primary_mouse_press_event(self, event):
|
||||
if event.button() == QtCore.Qt.LeftButton:
|
||||
self.primary_label_current_setting = (self.primary_label_current_setting + 1) % len(self.valid_cameras)
|
||||
self.set_max_resolutions_flag = True
|
||||
elif event.button() == QtCore.Qt.RightButton:
|
||||
if self.primary_label_current_setting in self.disabled_cameras:
|
||||
self.disabled_cameras.remove(self.primary_label_current_setting)
|
||||
else:
|
||||
self.disabled_cameras.append(self.primary_label_current_setting)
|
||||
self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].toggle_video_display()
|
||||
|
||||
def __change_display_source_secondary_mouse_press_event(self, event):
|
||||
if event.button() == QtCore.Qt.LeftButton:
|
||||
self.secondary_label_current_setting = (self.secondary_label_current_setting + 1) % len(self.valid_cameras)
|
||||
self.set_max_resolutions_flag = True
|
||||
elif event.button() == QtCore.Qt.RightButton:
|
||||
if self.secondary_label_current_setting in self.disabled_cameras:
|
||||
self.disabled_cameras.remove(self.secondary_label_current_setting)
|
||||
else:
|
||||
self.disabled_cameras.append(self.secondary_label_current_setting)
|
||||
self.camera_threads[self.valid_cameras[self.secondary_label_current_setting]].toggle_video_display()
|
||||
|
||||
def __change_display_source_tertiary_mouse_press_event(self, event):
|
||||
if event.button() == QtCore.Qt.LeftButton:
|
||||
self.tertiary_label_current_setting = (self.tertiary_label_current_setting + 1) % len(self.valid_cameras)
|
||||
self.set_max_resolutions_flag = True
|
||||
elif event.button() == QtCore.Qt.RightButton:
|
||||
if self.tertiary_label_current_setting in self.disabled_cameras:
|
||||
self.disabled_cameras.remove(self.tertiary_label_current_setting)
|
||||
else:
|
||||
self.disabled_cameras.append(self.tertiary_label_current_setting)
|
||||
self.camera_threads[self.valid_cameras[self.tertiary_label_current_setting]].toggle_video_display()
|
||||
|
||||
def pixmap_ready__slot(self, camera):
|
||||
if not self.first_image_received:
|
||||
self.first_image_received = True
|
||||
|
||||
if self.valid_cameras[self.primary_label_current_setting] == camera:
|
||||
self.primary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_1280x720_image)
|
||||
try:
|
||||
self.primary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_1280x720_image)
|
||||
except:
|
||||
pass
|
||||
|
||||
if self.valid_cameras[self.secondary_label_current_setting] == camera:
|
||||
self.secondary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
|
||||
try:
|
||||
self.secondary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
|
||||
except:
|
||||
pass
|
||||
|
||||
if self.valid_cameras[self.tertiary_label_current_setting] == camera:
|
||||
self.tertiary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
|
||||
try:
|
||||
self.tertiary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
|
||||
except:
|
||||
pass
|
||||
|
||||
def on_kill_threads_requested__slot(self):
|
||||
self.run_thread_flag = False
|
||||
|
||||
@@ -7,6 +7,7 @@ import logging
|
||||
import cv2
|
||||
import numpy as np
|
||||
import qimage2ndarray
|
||||
from time import time
|
||||
|
||||
import rospy
|
||||
import dynamic_reconfigure.client
|
||||
@@ -14,6 +15,7 @@ from cv_bridge import CvBridge
|
||||
from sensor_msgs.msg import CompressedImage
|
||||
|
||||
# Custom Imports
|
||||
from rover_camera.msg import CameraControlMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
@@ -23,6 +25,11 @@ CAMERA_TOPIC_PATH = "/cameras/"
|
||||
QUALITY_MAX = 80
|
||||
QUALITY_MIN = 15
|
||||
|
||||
FRAMERATE_AVERAGING_TIME = 10 # seconds
|
||||
|
||||
MIN_FRAMERATE_BEFORE_ADJUST = 23
|
||||
MAX_FRAMERATE_BEFORE_ADJUST = 28
|
||||
|
||||
|
||||
#####################################
|
||||
# RoverVideoReceiver Class Definition
|
||||
@@ -30,6 +37,14 @@ QUALITY_MIN = 15
|
||||
class RoverVideoReceiver(QtCore.QThread):
|
||||
image_ready_signal = QtCore.pyqtSignal(str)
|
||||
|
||||
RESOLUTION_OPTIONS = [(256, 144), (640, 360), (1280, 720)]
|
||||
|
||||
RESOLUTION_MAPPINGS = {
|
||||
(1280, 720): None,
|
||||
(640, 360): None,
|
||||
(256, 144): None
|
||||
}
|
||||
|
||||
def __init__(self, camera_name):
|
||||
super(RoverVideoReceiver, self).__init__()
|
||||
|
||||
@@ -49,22 +64,35 @@ class RoverVideoReceiver(QtCore.QThread):
|
||||
self.camera_title_name = self.camera_name.replace("_", " ").title()
|
||||
|
||||
self.topic_base_path = CAMERA_TOPIC_PATH + self.camera_name
|
||||
self.camera_topics = {}
|
||||
|
||||
self.current_max_resolution = None
|
||||
|
||||
self.current_camera_settings = {
|
||||
"resolution": None,
|
||||
"quality_setting": QUALITY_MAX,
|
||||
|
||||
}
|
||||
|
||||
self.previous_camera_settings = self.current_camera_settings.copy()
|
||||
|
||||
self.temp_topic_path = CAMERA_TOPIC_PATH + self.camera_name + "/image_640x360/compressed"
|
||||
self.control_topic_path = self.topic_base_path + "/camera_control"
|
||||
|
||||
# Subscription variables
|
||||
self.video_subscriber = None # type: rospy.Subscriber
|
||||
self.video_subscribers = []
|
||||
|
||||
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_1280x720/compressed", CompressedImage, self.__image_data_received_callback))
|
||||
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_640x360/compressed", CompressedImage, self.__image_data_received_callback))
|
||||
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_256x144/compressed", CompressedImage, self.__image_data_received_callback))
|
||||
|
||||
# Publisher variables
|
||||
self.camera_control_publisher = rospy.Publisher(self.control_topic_path, CameraControlMessage, queue_size=1)
|
||||
|
||||
# Set up resolution mappings
|
||||
self.RESOLUTION_MAPPINGS[(1280, 720)] = CameraControlMessage()
|
||||
self.RESOLUTION_MAPPINGS[(640, 360)] = CameraControlMessage()
|
||||
self.RESOLUTION_MAPPINGS[(256, 144)] = CameraControlMessage()
|
||||
|
||||
self.RESOLUTION_MAPPINGS[(1280, 720)].enable_large_broadcast = True
|
||||
self.RESOLUTION_MAPPINGS[(640, 360)].enable_medium_broadcast = True
|
||||
self.RESOLUTION_MAPPINGS[(256, 144)].enable_small_broadcast = True
|
||||
|
||||
# Auto resolution adjustment variables
|
||||
self.current_resolution_index = 1
|
||||
self.last_resolution_index = self.current_resolution_index
|
||||
self.max_resolution_index = len(self.RESOLUTION_OPTIONS)
|
||||
|
||||
self.frame_count = 0
|
||||
self.last_framerate_time = time()
|
||||
self.resolution_just_adjusted = False
|
||||
|
||||
# Image variables
|
||||
self.raw_image = None
|
||||
@@ -87,17 +115,14 @@ class RoverVideoReceiver(QtCore.QThread):
|
||||
self.camera_name_opencv_1280x720_image = None
|
||||
self.camera_name_opencv_640x360_image = None
|
||||
|
||||
# ROS Dynamic Reconfigure Client
|
||||
self.reconfigure_clients = {}
|
||||
|
||||
# Initial class setup to make text images and get camera resolutions
|
||||
self.__create_camera_name_opencv_images()
|
||||
self.__get_camera_available_resolutions()
|
||||
#self.__setup_reconfigure_clients()
|
||||
|
||||
def run(self):
|
||||
self.logger.debug("Starting \"%s\" Camera Thread" % self.camera_title_name)
|
||||
|
||||
self.__enable_camera_resolution(self.RESOLUTION_OPTIONS[self.current_resolution_index])
|
||||
|
||||
while self.run_thread_flag:
|
||||
if self.video_enabled:
|
||||
self.__show_video_enabled()
|
||||
@@ -108,62 +133,43 @@ class RoverVideoReceiver(QtCore.QThread):
|
||||
|
||||
self.logger.debug("Stopping \"%s\" Camera Thread" % self.camera_title_name)
|
||||
|
||||
def __perform_quality_check_and_adjust(self):
|
||||
self.__set_jpeg_quality(self.current_camera_settings["quality_setting"])
|
||||
def __enable_camera_resolution(self, resolution):
|
||||
self.camera_control_publisher.publish(self.RESOLUTION_MAPPINGS[resolution])
|
||||
|
||||
def __set_jpeg_quality(self, quality_setting):
|
||||
self.reconfigure_clients[self.current_camera_settings["resolution"]].update_configuration({"jpeg_quality": quality_setting})
|
||||
def __check_framerate_and_adjust_resolution(self):
|
||||
time_diff = time() - self.last_framerate_time
|
||||
if time_diff > FRAMERATE_AVERAGING_TIME:
|
||||
current_fps = self.frame_count / time_diff
|
||||
|
||||
def __setup_reconfigure_clients(self):
|
||||
for resolution_group in self.camera_topics:
|
||||
image_topic_string = "image_%sx%s" % resolution_group
|
||||
full_topic = self.topic_base_path + "/" + image_topic_string + "/compressed"
|
||||
self.reconfigure_clients[resolution_group] = dynamic_reconfigure.client.Client(full_topic)
|
||||
if current_fps >= MAX_FRAMERATE_BEFORE_ADJUST:
|
||||
self.current_resolution_index = min(self.current_resolution_index + 1, self.max_resolution_index)
|
||||
elif current_fps <= MIN_FRAMERATE_BEFORE_ADJUST:
|
||||
self.current_resolution_index = max(self.current_resolution_index - 1, 0)
|
||||
|
||||
def __get_camera_available_resolutions(self):
|
||||
topics = rospy.get_published_topics(self.topic_base_path)
|
||||
if self.last_resolution_index != self.current_resolution_index:
|
||||
self.camera_control_publisher.publish(
|
||||
self.RESOLUTION_MAPPINGS[self.RESOLUTION_OPTIONS[self.current_resolution_index]])
|
||||
print "Setting %s to %s" % (self.camera_title_name, self.RESOLUTION_OPTIONS[self.current_resolution_index])
|
||||
self.last_resolution_index = self.current_resolution_index
|
||||
self.resolution_just_adjusted = True
|
||||
|
||||
resolution_options = []
|
||||
|
||||
for topics_group in topics:
|
||||
main_topic = topics_group[0]
|
||||
if "heartbeat" in main_topic:
|
||||
continue
|
||||
camera_name = main_topic.split("/")[3]
|
||||
resolution_options.append(camera_name)
|
||||
|
||||
resolution_options = list(set(resolution_options))
|
||||
|
||||
for resolution in resolution_options:
|
||||
# Creates a tuple in (width, height) format that we can use as the key
|
||||
group = int(resolution.split("image_")[1].split("x")[0]), int(resolution.split("image_")[1].split("x")[1])
|
||||
self.camera_topics[group] = self.topic_base_path + "/" + resolution + "/compressed"
|
||||
|
||||
def __update_camera_subscription_and_settings(self):
|
||||
if self.current_camera_settings["resolution"] != self.previous_camera_settings["resolution"]:
|
||||
|
||||
if self.video_subscriber:
|
||||
self.video_subscriber.unregister()
|
||||
new_topic = self.camera_topics[self.current_camera_settings["resolution"]]
|
||||
self.video_subscriber = rospy.Subscriber(new_topic, CompressedImage, self.__image_data_received_callback)
|
||||
|
||||
self.new_frame = False
|
||||
while not self.new_frame:
|
||||
self.msleep(10)
|
||||
|
||||
self.previous_camera_settings["resolution"] = self.current_camera_settings["resolution"]
|
||||
# print "%s: %s FPS" % (self.camera_title_name, current_fps)
|
||||
self.last_framerate_time = time()
|
||||
self.frame_count = 0
|
||||
|
||||
def __show_video_enabled(self):
|
||||
self.__update_camera_subscription_and_settings()
|
||||
if self.new_frame:
|
||||
self.__check_framerate_and_adjust_resolution()
|
||||
|
||||
if self.new_frame and self.current_camera_settings["resolution"]:
|
||||
# self.__perform_quality_check_and_adjust()
|
||||
try:
|
||||
opencv_image = self.bridge.compressed_imgmsg_to_cv2(self.raw_image, "rgb8")
|
||||
|
||||
opencv_image = self.bridge.compressed_imgmsg_to_cv2(self.raw_image, "rgb8")
|
||||
self.__create_final_pixmaps(opencv_image)
|
||||
|
||||
self.__create_final_pixmaps(opencv_image)
|
||||
self.image_ready_signal.emit(self.camera_name)
|
||||
except Exception, error:
|
||||
print "Failed with error:" + str(error)
|
||||
|
||||
self.image_ready_signal.emit(self.camera_name)
|
||||
self.new_frame = False
|
||||
|
||||
def __show_video_disabled(self):
|
||||
@@ -196,8 +202,14 @@ class RoverVideoReceiver(QtCore.QThread):
|
||||
|
||||
def __image_data_received_callback(self, raw_image):
|
||||
self.raw_image = raw_image
|
||||
self.frame_count += 1
|
||||
self.new_frame = True
|
||||
|
||||
if self.resolution_just_adjusted:
|
||||
self.frame_count = 0
|
||||
self.last_framerate_time = time()
|
||||
self.resolution_just_adjusted = False
|
||||
|
||||
def __create_camera_name_opencv_images(self):
|
||||
camera_name_text_width, camera_name_text_height = \
|
||||
cv2.getTextSize(self.camera_title_name, self.font, self.font_thickness, self.font_baseline)[0]
|
||||
@@ -224,20 +236,17 @@ class RoverVideoReceiver(QtCore.QThread):
|
||||
self.camera_name_opencv_640x360_image = \
|
||||
cv2.resize(camera_name_opencv_image, (camera_name_width_buffered / 2, camera_name_height_buffered / 2))
|
||||
|
||||
def change_max_resolution_setting(self, resolution_max):
|
||||
self.current_max_resolution = resolution_max
|
||||
self.current_camera_settings["resolution"] = resolution_max
|
||||
def set_hard_max_resolution(self, resolution):
|
||||
self.max_resolution_index = self.RESOLUTION_OPTIONS.index(resolution)
|
||||
|
||||
def toggle_video_display(self):
|
||||
if self.video_enabled:
|
||||
if self.video_subscriber:
|
||||
self.video_subscriber.unregister()
|
||||
self.new_frame = True
|
||||
self.video_enabled = False
|
||||
else:
|
||||
new_topic = self.camera_topics[self.current_camera_settings["resolution"]]
|
||||
self.video_subscriber = rospy.Subscriber(new_topic, CompressedImage, self.__image_data_received_callback)
|
||||
if not self.video_enabled:
|
||||
self.camera_control_publisher.publish(self.RESOLUTION_MAPPINGS[self.RESOLUTION_OPTIONS[self.current_resolution_index]])
|
||||
self.video_enabled = True
|
||||
self.new_frame = True
|
||||
else:
|
||||
self.camera_control_publisher.publish(CameraControlMessage())
|
||||
self.video_enabled = False
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 144 KiB |
@@ -42,7 +42,16 @@
|
||||
<property name="sizeConstraint">
|
||||
<enum>QLayout::SetDefaultConstraint</enum>
|
||||
</property>
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -75,9 +84,453 @@
|
||||
<height>720</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color:black;</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_44">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_9">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_44">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Arm Joint Positions</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="horizontalSpacer_6">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_10">
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_39">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_45">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>14</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Base Rotation</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QDial" name="dial_31">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>305</width>
|
||||
<height>185</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>-180</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>180</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="sliderPosition">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="tracking">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="invertedAppearance">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="invertedControls">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="wrapping">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="notchTarget">
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_40">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_46">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>14</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Shoulder Pitch</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QDial" name="dial_32">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>305</width>
|
||||
<height>185</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>-180</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>180</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="sliderPosition">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="tracking">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="invertedAppearance">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="invertedControls">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="wrapping">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="notchTarget">
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_11">
|
||||
<item>
|
||||
<spacer name="horizontalSpacer_7">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_41">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_47">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>14</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Elbow Pitch</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QDial" name="dial_33">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>305</width>
|
||||
<height>185</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>-180</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>180</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="sliderPosition">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="tracking">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="invertedAppearance">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="invertedControls">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="wrapping">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="notchTarget">
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="horizontalSpacer_8">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_12">
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_42">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_48">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>14</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>End Effector Pitch</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QDial" name="dial_34">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>305</width>
|
||||
<height>185</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>-180</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>180</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="sliderPosition">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="tracking">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="invertedAppearance">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="invertedControls">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="wrapping">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="notchTarget">
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_43">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_49">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>14</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>End Effector Rotation</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QDial" name="dial_35">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>305</width>
|
||||
<height>185</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>-180</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>180</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="sliderPosition">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="tracking">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="invertedAppearance">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="invertedControls">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="wrapping">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="notchTarget">
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="verticalSpacer">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>3</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="Line" name="line">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
@@ -108,26 +561,207 @@
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color:black;;</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout">
|
||||
<property name="margin">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="compass_label">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
<item row="0" column="1">
|
||||
<widget class="Line" name="line_2">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="0">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_5">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout_2">
|
||||
<property name="leftMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="current_heading_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>14</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>260°</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<spacer name="horizontalSpacer_2">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_8">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>17</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Heading</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="2">
|
||||
<widget class="QLabel" name="label_10">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>17</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Next Goal</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<widget class="QLabel" name="next_goal_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>14</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>258°</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="verticalSpacer_6">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_7">
|
||||
<item>
|
||||
<spacer name="horizontalSpacer_4">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="heading_compass_label">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>275</width>
|
||||
<height>275</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>275</width>
|
||||
<height>275</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="scaledContents">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="horizontalSpacer_5">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="verticalSpacer_5">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
@@ -152,6 +786,21 @@
|
||||
</size>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QLabel" name="label">
|
||||
<property name="font">
|
||||
@@ -170,19 +819,51 @@
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
</font>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_2">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>2.4 m/s</string>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>0.0</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_7">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>m/s</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="verticalSpacer_3">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</widget>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_3">
|
||||
@@ -202,13 +883,19 @@
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QProgressBar" name="progressBar">
|
||||
<widget class="QProgressBar" name="speed_limit_progress_bar">
|
||||
<property name="maximum">
|
||||
<number>100</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>50</number>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
<property name="textVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
@@ -220,6 +907,19 @@
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="verticalSpacer_4">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_4">
|
||||
<property name="font">
|
||||
@@ -238,18 +938,88 @@
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QProgressBar" name="progressBar_3">
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
<layout class="QFormLayout" name="formLayout">
|
||||
<property name="horizontalSpacing">
|
||||
<number>6</number>
|
||||
</property>
|
||||
</widget>
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_5">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string> Left</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QProgressBar" name="left_drive_progress_bar">
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color:darkred;</string>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>100</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="textVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_6">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Right</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QProgressBar" name="right_drive_progress_bar">
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color:darkred;</string>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>100</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="textVisible">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QProgressBar" name="progressBar_2">
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
<spacer name="verticalSpacer_2">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
</widget>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
|
||||
@@ -98,14 +98,14 @@ class GroundStation(QtCore.QObject):
|
||||
|
||||
# ##### Instantiate Threaded Classes ######
|
||||
self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
|
||||
self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects))
|
||||
# self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects))
|
||||
self.__add_thread("Rover Drive Sender", RoverDriveSender.RoverDriveSender(self.shared_objects))
|
||||
self.connect_signals_and_slots_signal.emit()
|
||||
self.__connect_signals_to_slots()
|
||||
self.start_threads_signal.emit()
|
||||
|
||||
compass_image = PIL.Image.open("Resources/Images/compass.png").resize((300, 300)) # PIL.Image
|
||||
self.shared_objects["screens"]["right_screen"].compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image)))
|
||||
self.shared_objects["screens"]["right_screen"].heading_compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image)))
|
||||
|
||||
def ___ros_master_running(self):
|
||||
checker = ROSMasterChecker.ROSMasterChecker()
|
||||
|
||||
@@ -1,5 +0,0 @@
|
||||
topics:
|
||||
- name: "/drive/motoroneandtwo"
|
||||
compress: true # enable bz2 compression
|
||||
rate: 20.0 # rate limit at 1Hz
|
||||
- name: "/my_second_topic"
|
||||
@@ -3,9 +3,24 @@ project(rover_camera)
|
||||
|
||||
add_compile_options(-std=c++11)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport message_generation sensor_msgs)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
cv_bridge
|
||||
image_transport
|
||||
message_generation
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
)
|
||||
|
||||
# add the resized image message
|
||||
add_message_files(
|
||||
FILES
|
||||
CameraControlMessage.msg
|
||||
)
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
)
|
||||
|
||||
catkin_package(CATKIN_DEPENDS cv_bridge image_transport message_runtime sensor_msgs)
|
||||
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
bool enable_small_broadcast
|
||||
bool enable_medium_broadcast
|
||||
bool enable_large_broadcast
|
||||
@@ -5,15 +5,153 @@
|
||||
#include <ros/console.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
#include "rover_camera/CameraControlMessage.h"
|
||||
|
||||
// Useful info
|
||||
// RTSP stream is 704x480
|
||||
// 3 image channels
|
||||
// Image type 16
|
||||
|
||||
class RoverCamera{
|
||||
public:
|
||||
RoverCamera(int argc, char** argv){
|
||||
ros::init(argc, argv, "camera");
|
||||
|
||||
node_handle = new ros::NodeHandle("~");
|
||||
|
||||
node_handle->param("is_rtsp_camera", is_rtsp_camera, false);
|
||||
node_handle->param("device_path", capture_device_path, std::string("/dev/video0"));
|
||||
node_handle->param("fps", fps, 30);
|
||||
|
||||
node_handle->param("large_image_width", large_image_width, 1280);
|
||||
node_handle->param("large_image_height", large_image_height, 720);
|
||||
node_handle->param("medium_image_width", medium_image_width, 640);
|
||||
node_handle->param("medium_image_height", medium_image_height, 360);
|
||||
node_handle->param("small_image_width", small_image_width, 256);
|
||||
node_handle->param("small_image_height", small_image_height, 144);
|
||||
|
||||
broadcast_large_image = false;
|
||||
broadcast_medium_image = false;
|
||||
broadcast_small_image = true;
|
||||
|
||||
if(is_rtsp_camera){
|
||||
cap = new cv::VideoCapture(capture_device_path);
|
||||
ROS_INFO_STREAM("Connecting to RTSP camera with path: " << capture_device_path);
|
||||
|
||||
}else{
|
||||
cap = new cv::VideoCapture(capture_device_path);
|
||||
ROS_INFO_STREAM("Connecting to USB camera with path: " << capture_device_path);
|
||||
|
||||
cap->set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
|
||||
cap->set(CV_CAP_PROP_FRAME_WIDTH, large_image_width);
|
||||
cap->set(CV_CAP_PROP_FRAME_HEIGHT, large_image_height);
|
||||
cap->set(CV_CAP_PROP_FPS, fps);
|
||||
}
|
||||
|
||||
large_image_node_name = "image_" + std::to_string(large_image_width) + "x" + std::to_string(large_image_height);
|
||||
medium_image_node_name = "image_" + std::to_string(medium_image_width) + "x" + std::to_string(medium_image_height);
|
||||
small_image_node_name = "image_" + std::to_string(small_image_width) + "x" + std::to_string(small_image_height);
|
||||
|
||||
large_image_transport = new image_transport::ImageTransport(*node_handle);
|
||||
medium_image_transport = new image_transport::ImageTransport(*node_handle);
|
||||
small_image_transport = new image_transport::ImageTransport(*node_handle);
|
||||
|
||||
large_image_publisher = large_image_transport->advertise(large_image_node_name, 1);
|
||||
medium_image_publisher = medium_image_transport->advertise(medium_image_node_name, 1);
|
||||
small_image_publisher = small_image_transport->advertise(small_image_node_name, 1);
|
||||
|
||||
control_subscriber = node_handle->subscribe("camera_control", 1, &RoverCamera::control_callback, this);
|
||||
|
||||
if(is_rtsp_camera){
|
||||
loop_rate = new ros::Rate(fps + 10);
|
||||
}else{
|
||||
loop_rate = new ros::Rate(fps + 2);
|
||||
}
|
||||
|
||||
image_black = cv::Mat(large_image_height, large_image_width, CV_8UC3, CvScalar(0, 0, 0));
|
||||
}
|
||||
|
||||
void run(){
|
||||
if(!cap->isOpened()){
|
||||
return;
|
||||
}
|
||||
|
||||
while (ros::ok()) {
|
||||
|
||||
if(!is_rtsp_camera) {
|
||||
cap->read(image_large);
|
||||
}else{
|
||||
cap->read(image_rtsp_raw);
|
||||
image_black.copyTo(image_large);
|
||||
|
||||
float image_scalar = float(image_large.rows) / image_rtsp_raw.rows;
|
||||
|
||||
cv::resize(image_rtsp_raw, image_rtsp_scaled, CvSize(int(image_rtsp_raw.cols * image_scalar), int(image_rtsp_raw.rows * image_scalar)));
|
||||
|
||||
int x = (image_large.cols - image_rtsp_scaled.cols) / 2;
|
||||
|
||||
image_rtsp_scaled.copyTo(image_large(CvRect(x , 0, image_rtsp_scaled.cols, image_rtsp_scaled.rows)));
|
||||
}
|
||||
|
||||
if(!image_large.empty()){
|
||||
bool large_ready_to_broadcast = false;
|
||||
bool medium_ready_to_broadcast = false;
|
||||
bool small_ready_to_broadcast = false;
|
||||
|
||||
if(broadcast_large_image){
|
||||
large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg();
|
||||
large_ready_to_broadcast = true;
|
||||
}
|
||||
|
||||
if(broadcast_medium_image){
|
||||
cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height));
|
||||
medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg();
|
||||
medium_ready_to_broadcast = true;
|
||||
}
|
||||
|
||||
if(broadcast_small_image){
|
||||
cv::resize(image_large, image_small, cv::Size(small_image_width, small_image_height));
|
||||
small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg();
|
||||
small_ready_to_broadcast = true;
|
||||
}
|
||||
|
||||
if(large_ready_to_broadcast){ large_image_publisher.publish(large_image_message); };
|
||||
if(medium_ready_to_broadcast){ medium_image_publisher.publish(medium_image_message); };
|
||||
if(small_ready_to_broadcast){ small_image_publisher.publish(small_image_message); };
|
||||
}
|
||||
|
||||
ros::spinOnce();
|
||||
loop_rate->sleep();
|
||||
}
|
||||
}
|
||||
|
||||
void control_callback(const rover_camera::CameraControlMessage::ConstPtr& msg){
|
||||
broadcast_small_image = msg->enable_small_broadcast;
|
||||
broadcast_medium_image = msg->enable_medium_broadcast;
|
||||
broadcast_large_image = msg->enable_large_broadcast;
|
||||
}
|
||||
|
||||
~RoverCamera(){
|
||||
if(cap->isOpened()){
|
||||
cap->release();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
ros::NodeHandle *node_handle;
|
||||
|
||||
cv::VideoCapture *cap;
|
||||
|
||||
bool is_rtsp_camera;
|
||||
|
||||
std::string capture_device_path;
|
||||
int fps;
|
||||
|
||||
ros::Rate *loop_rate;
|
||||
|
||||
int large_image_width;
|
||||
int large_image_height;
|
||||
int medium_image_width;
|
||||
@@ -21,68 +159,40 @@ int main(int argc, char** argv)
|
||||
int small_image_width;
|
||||
int small_image_height;
|
||||
|
||||
ros::init(argc, argv, "camera");
|
||||
ros::NodeHandle node_handle("~");
|
||||
bool broadcast_large_image;
|
||||
bool broadcast_medium_image;
|
||||
bool broadcast_small_image;
|
||||
|
||||
node_handle.param("device_path", capture_device_path, std::string("/dev/video0"));
|
||||
node_handle.param("fps", fps, 30);
|
||||
std::string large_image_node_name;
|
||||
std::string medium_image_node_name;
|
||||
std::string small_image_node_name;
|
||||
|
||||
node_handle.param("large_image_width", large_image_width, 1280);
|
||||
node_handle.param("large_image_height", large_image_height, 720);
|
||||
node_handle.param("medium_image_width", medium_image_width, 640);
|
||||
node_handle.param("medium_image_height", medium_image_height, 360);
|
||||
node_handle.param("small_image_width", small_image_width, 256);
|
||||
node_handle.param("small_image_height", small_image_height, 144);
|
||||
image_transport::ImageTransport *large_image_transport;
|
||||
image_transport::ImageTransport *medium_image_transport;
|
||||
image_transport::ImageTransport *small_image_transport;
|
||||
|
||||
cv::VideoCapture cap(capture_device_path);
|
||||
image_transport::Publisher large_image_publisher;
|
||||
image_transport::Publisher medium_image_publisher;
|
||||
image_transport::Publisher small_image_publisher;
|
||||
|
||||
cap.set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
|
||||
cap.set(CV_CAP_PROP_FRAME_WIDTH, large_image_width);
|
||||
cap.set(CV_CAP_PROP_FRAME_HEIGHT, large_image_height);
|
||||
cap.set(CV_CAP_PROP_FPS, fps);
|
||||
ros::Subscriber control_subscriber;
|
||||
|
||||
if(cap.isOpened() == false){
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::string large_image_node_name = "image_" + std::to_string(large_image_width) + "x" + std::to_string(large_image_height);
|
||||
std::string medium_image_node_name = "image_" + std::to_string(medium_image_width) + "x" + std::to_string(medium_image_height);
|
||||
std::string small_image_node_name = "image_" + std::to_string(small_image_width) + "x" + std::to_string(small_image_height);
|
||||
|
||||
image_transport::ImageTransport large_image_transport(node_handle);
|
||||
image_transport::ImageTransport medium_image_transport(node_handle);
|
||||
image_transport::ImageTransport small_image_transport(node_handle);
|
||||
|
||||
image_transport::Publisher large_image_publisher = large_image_transport.advertise(large_image_node_name, 1);
|
||||
image_transport::Publisher medium_image_publisher = medium_image_transport.advertise(medium_image_node_name, 1);
|
||||
image_transport::Publisher small_image_publisher = small_image_transport.advertise(small_image_node_name, 1);
|
||||
cv::Mat image_black;
|
||||
|
||||
cv::Mat image_rtsp_raw;
|
||||
cv::Mat image_rtsp_scaled;
|
||||
cv::Mat image_large;
|
||||
cv::Mat image_medium;
|
||||
cv::Mat image_small;
|
||||
|
||||
ros::Rate loop_rate(fps + 2);
|
||||
sensor_msgs::ImagePtr large_image_message;
|
||||
sensor_msgs::ImagePtr medium_image_message;
|
||||
sensor_msgs::ImagePtr small_image_message;
|
||||
|
||||
while (ros::ok()) {
|
||||
};
|
||||
|
||||
cap.read(image_large);
|
||||
|
||||
if(!image_large.empty()){
|
||||
cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height));
|
||||
cv::resize(image_medium, image_small, cv::Size(small_image_width, small_image_height));
|
||||
|
||||
sensor_msgs::ImagePtr large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg();
|
||||
sensor_msgs::ImagePtr medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg();
|
||||
sensor_msgs::ImagePtr small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg();
|
||||
|
||||
large_image_publisher.publish(large_image_message);
|
||||
medium_image_publisher.publish(medium_image_message);
|
||||
small_image_publisher.publish(small_image_message);
|
||||
}
|
||||
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
cap.release();
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
RoverCamera camera(argc, argv);
|
||||
camera.run();
|
||||
}
|
||||
|
||||
88
software/ros_packages/rover_camera/src/rover_camera_old.cpp
Normal file
88
software/ros_packages/rover_camera/src/rover_camera_old.cpp
Normal file
@@ -0,0 +1,88 @@
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <ros/console.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
std::string capture_device_path;
|
||||
int fps;
|
||||
|
||||
int large_image_width;
|
||||
int large_image_height;
|
||||
int medium_image_width;
|
||||
int medium_image_height;
|
||||
int small_image_width;
|
||||
int small_image_height;
|
||||
|
||||
ros::init(argc, argv, "camera");
|
||||
ros::NodeHandle node_handle("~");
|
||||
|
||||
node_handle.param("device_path", capture_device_path, std::string("/dev/video0"));
|
||||
node_handle.param("fps", fps, 30);
|
||||
|
||||
node_handle.param("large_image_width", large_image_width, 1280);
|
||||
node_handle.param("large_image_height", large_image_height, 720);
|
||||
node_handle.param("medium_image_width", medium_image_width, 640);
|
||||
node_handle.param("medium_image_height", medium_image_height, 360);
|
||||
node_handle.param("small_image_width", small_image_width, 256);
|
||||
node_handle.param("small_image_height", small_image_height, 144);
|
||||
|
||||
cv::VideoCapture cap(capture_device_path);
|
||||
|
||||
cap.set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
|
||||
cap.set(CV_CAP_PROP_FRAME_WIDTH, large_image_width);
|
||||
cap.set(CV_CAP_PROP_FRAME_HEIGHT, large_image_height);
|
||||
cap.set(CV_CAP_PROP_FPS, fps);
|
||||
|
||||
if(!cap.isOpened()){
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::string large_image_node_name = "image_" + std::to_string(large_image_width) + "x" + std::to_string(large_image_height);
|
||||
std::string medium_image_node_name = "image_" + std::to_string(medium_image_width) + "x" + std::to_string(medium_image_height);
|
||||
std::string small_image_node_name = "image_" + std::to_string(small_image_width) + "x" + std::to_string(small_image_height);
|
||||
|
||||
image_transport::ImageTransport large_image_transport(node_handle);
|
||||
image_transport::ImageTransport medium_image_transport(node_handle);
|
||||
image_transport::ImageTransport small_image_transport(node_handle);
|
||||
|
||||
image_transport::Publisher large_image_publisher = large_image_transport.advertise(large_image_node_name, 1);
|
||||
image_transport::Publisher medium_image_publisher = medium_image_transport.advertise(medium_image_node_name, 1);
|
||||
image_transport::Publisher small_image_publisher = small_image_transport.advertise(small_image_node_name, 1);
|
||||
|
||||
cv::Mat image_large;
|
||||
cv::Mat image_medium;
|
||||
cv::Mat image_small;
|
||||
|
||||
ros::Rate loop_rate(fps + 2);
|
||||
|
||||
while (ros::ok()) {
|
||||
|
||||
cap.read(image_large);
|
||||
|
||||
if(!image_large.empty()){
|
||||
cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height));
|
||||
cv::resize(image_medium, image_small, cv::Size(small_image_width, small_image_height));
|
||||
|
||||
sensor_msgs::ImagePtr large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg();
|
||||
sensor_msgs::ImagePtr medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg();
|
||||
sensor_msgs::ImagePtr small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg();
|
||||
|
||||
large_image_publisher.publish(large_image_message);
|
||||
medium_image_publisher.publish(medium_image_message);
|
||||
small_image_publisher.publish(small_image_message);
|
||||
}
|
||||
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
cap.release();
|
||||
}
|
||||
@@ -52,6 +52,7 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
FILES
|
||||
DriveCommandMessage.msg
|
||||
DriveControlMessage.msg
|
||||
DriveStatusMessage.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
|
||||
@@ -1,2 +1,3 @@
|
||||
bool controller_present
|
||||
bool ignore_drive_control
|
||||
geometry_msgs/Twist drive_twist
|
||||
@@ -0,0 +1,11 @@
|
||||
bool first_motor_connected
|
||||
bool second_motor_connected
|
||||
|
||||
float32 first_motor_current
|
||||
float32 second_motor_current
|
||||
|
||||
bool first_motor_fault
|
||||
bool second_motor_fault
|
||||
|
||||
float32 first_motor_temp
|
||||
float32 second_motor_temp
|
||||
@@ -23,7 +23,9 @@ DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right"
|
||||
|
||||
UINT16_MAX = 65535
|
||||
|
||||
DEFAULT_HERTZ = 15
|
||||
DEFAULT_HERTZ = 30
|
||||
|
||||
WATCHDOG_TIMEOUT = 0.3
|
||||
|
||||
|
||||
#####################################
|
||||
@@ -56,12 +58,20 @@ class DriveCoordinator(object):
|
||||
self.left_bogie_publisher = rospy.Publisher(self.left_bogie_topic, DriveControlMessage, queue_size=1)
|
||||
self.right_bogie_publisher = rospy.Publisher(self.right_bogie_topic, DriveControlMessage, queue_size=1)
|
||||
|
||||
# Other Vars TODO: fix this later
|
||||
self.drive_commands = {
|
||||
"iris": DriveCommandMessage(),
|
||||
"ground_station": DriveCommandMessage()
|
||||
self.drive_command_data = {
|
||||
"iris": {
|
||||
"message": DriveCommandMessage(),
|
||||
"last_time": time()
|
||||
},
|
||||
|
||||
"ground_station": {
|
||||
"message": DriveCommandMessage(),
|
||||
"last_time": time()
|
||||
}
|
||||
}
|
||||
|
||||
self.last_message_time = time()
|
||||
|
||||
# ########## Run the Class ##########
|
||||
self.run()
|
||||
|
||||
@@ -79,12 +89,18 @@ class DriveCoordinator(object):
|
||||
sleep(max(self.wait_time - time_diff, 0))
|
||||
|
||||
def process_drive_commands(self):
|
||||
if not self.drive_commands["iris"].ignore_drive_control:
|
||||
self.send_drive_control_command(self.drive_commands["iris"])
|
||||
if not self.drive_command_data["iris"]["message"].ignore_drive_control:
|
||||
self.send_drive_control_command(self.drive_command_data["iris"])
|
||||
else:
|
||||
self.send_drive_control_command(self.drive_commands["ground_station"])
|
||||
self.send_drive_control_command(self.drive_command_data["ground_station"])
|
||||
|
||||
def send_drive_control_command(self, drive_command_data):
|
||||
|
||||
if (time() - drive_command_data["last_time"]) > WATCHDOG_TIMEOUT:
|
||||
drive_command = DriveCommandMessage()
|
||||
else:
|
||||
drive_command = drive_command_data["message"]
|
||||
|
||||
def send_drive_control_command(self, drive_command):
|
||||
rear_drive = DriveControlMessage()
|
||||
left_drive = DriveControlMessage()
|
||||
right_drive = DriveControlMessage()
|
||||
@@ -117,14 +133,13 @@ class DriveCoordinator(object):
|
||||
self.left_bogie_publisher.publish(left_drive)
|
||||
self.right_bogie_publisher.publish(right_drive)
|
||||
|
||||
|
||||
def iris_drive_command_callback(self, drive_command):
|
||||
self.drive_commands["iris"] = drive_command
|
||||
return
|
||||
self.drive_command_data["iris"]["message"] = drive_command
|
||||
self.drive_command_data["iris"]["last_time"] = time()
|
||||
|
||||
def ground_station_drive_command_callback(self, drive_command):
|
||||
self.drive_commands["ground_station"] = drive_command
|
||||
return
|
||||
self.drive_command_data["ground_station"]["message"] = drive_command
|
||||
self.drive_command_data["ground_station"]["last_time"] = time()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
@@ -11,7 +11,7 @@ import serial.rs485
|
||||
import minimalmodbus
|
||||
|
||||
# Custom Imports
|
||||
from rover_control.msg import DriveControlMessage
|
||||
from rover_control.msg import DriveControlMessage, DriveStatusMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
@@ -24,15 +24,18 @@ DEFAULT_BAUD = 115200
|
||||
DEFAULT_INVERT = False
|
||||
|
||||
DEFAULT_DRIVE_CONTROL_TOPIC = "drive_control/rear"
|
||||
DEFAULT_DRIVE_CONTROL_STATUS_TOPIC = "drive_status/rear"
|
||||
|
||||
FIRST_MOTOR_ID = 1
|
||||
SECOND_MOTOR_ID = 2
|
||||
|
||||
COMMUNICATIONS_TIMEOUT = 0.15 # Seconds
|
||||
COMMUNICATIONS_TIMEOUT = 0.01 # Seconds
|
||||
|
||||
RX_DELAY = 0.01
|
||||
TX_DELAY = 0.01
|
||||
|
||||
DEFAULT_HERTZ = 20
|
||||
|
||||
MODBUS_REGISTERS = {
|
||||
"DIRECTION": 0,
|
||||
"SPEED": 1,
|
||||
@@ -52,6 +55,7 @@ MOTOR_DRIVER_DEFAULT_MESSAGE = [
|
||||
|
||||
UINT16_MAX = 65535
|
||||
|
||||
|
||||
#####################################
|
||||
# DriveControl Class Definition
|
||||
#####################################
|
||||
@@ -67,6 +71,10 @@ class DriveControl(object):
|
||||
|
||||
self.drive_control_subscriber_topic = rospy.get_param("~drive_control_topic", DEFAULT_DRIVE_CONTROL_TOPIC)
|
||||
|
||||
self.drive_control_status_topic = rospy.get_param("~drive_control_status_topic", DEFAULT_DRIVE_CONTROL_STATUS_TOPIC)
|
||||
|
||||
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
|
||||
|
||||
self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID)
|
||||
self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID)
|
||||
self.__setup_minimalmodbus_for_485()
|
||||
@@ -74,6 +82,11 @@ class DriveControl(object):
|
||||
self.drive_control_subscriber = \
|
||||
rospy.Subscriber(self.drive_control_subscriber_topic, DriveControlMessage, self.drive_control_callback)
|
||||
|
||||
self.drive_control_status_publisher = rospy.Publisher(self.drive_control_status_topic, DriveStatusMessage, queue_size=1)
|
||||
|
||||
self.drive_control_message = DriveControlMessage()
|
||||
self.new_control_message = False
|
||||
|
||||
self.run()
|
||||
|
||||
def __setup_minimalmodbus_for_485(self):
|
||||
@@ -89,27 +102,76 @@ class DriveControl(object):
|
||||
|
||||
def run(self):
|
||||
while not rospy.is_shutdown():
|
||||
sleep(0.25)
|
||||
start_time = time()
|
||||
|
||||
try:
|
||||
self.send_drive_control_message()
|
||||
self.get_drive_status()
|
||||
|
||||
except Exception, error:
|
||||
print "Error occurred:", error
|
||||
|
||||
time_diff = time() - start_time
|
||||
|
||||
sleep(max(self.wait_time - time_diff, 0))
|
||||
|
||||
def send_drive_control_message(self):
|
||||
if self.new_control_message:
|
||||
drive_control = self.drive_control_message
|
||||
try:
|
||||
first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
|
||||
first_direction = \
|
||||
not drive_control.first_motor_direction if self.first_motor_inverted else drive_control.first_motor_direction
|
||||
first_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = first_direction
|
||||
first_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.first_motor_speed, UINT16_MAX)
|
||||
|
||||
second_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
|
||||
second_direction = not drive_control.second_motor_direction if self.second_motor_inverted else drive_control.second_motor_direction
|
||||
second_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = second_direction
|
||||
second_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.second_motor_speed, UINT16_MAX)
|
||||
|
||||
self.first_motor.write_registers(0, first_motor_register_data)
|
||||
self.second_motor.write_registers(0, second_motor_register_data)
|
||||
|
||||
except Exception, error:
|
||||
print "Error occurred:", error
|
||||
|
||||
self.new_control_message = False
|
||||
|
||||
def get_drive_status(self):
|
||||
status = DriveStatusMessage()
|
||||
|
||||
first_motor_status = [0, 0, 0]
|
||||
second_motor_status = [0, 0, 0]
|
||||
|
||||
try:
|
||||
first_motor_status = self.first_motor.read_registers(3, 3)
|
||||
status.first_motor_connected = True
|
||||
except Exception:
|
||||
status.first_motor_connected = False
|
||||
|
||||
try:
|
||||
second_motor_status = self.second_motor.read_registers(3, 3)
|
||||
status.second_motor_connected = True
|
||||
except Exception:
|
||||
status.second_motor_connected = False
|
||||
|
||||
if status.first_motor_connected:
|
||||
status.first_motor_current = first_motor_status[0] / 1000.0
|
||||
status.first_motor_fault = first_motor_status[1]
|
||||
status.first_motor_temp = first_motor_status[2] / 1000.0
|
||||
|
||||
if status.second_motor_connected:
|
||||
status.second_motor_current = second_motor_status[0] / 1000.0
|
||||
status.second_motor_fault = second_motor_status[1]
|
||||
status.second_motor_temp = second_motor_status[2] / 1000.0
|
||||
|
||||
self.drive_control_status_publisher.publish(status)
|
||||
|
||||
def drive_control_callback(self, drive_control):
|
||||
try:
|
||||
first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
|
||||
first_direction = \
|
||||
not drive_control.first_motor_direction if self.first_motor_inverted else drive_control.first_motor_direction
|
||||
first_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = first_direction
|
||||
first_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.first_motor_speed, UINT16_MAX)
|
||||
|
||||
second_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
|
||||
second_direction = not drive_control.second_motor_direction if self.second_motor_inverted else drive_control.second_motor_direction
|
||||
second_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = second_direction
|
||||
second_motor_register_data[MODBUS_REGISTERS["SPEED"]] = min(drive_control.second_motor_speed, UINT16_MAX)
|
||||
|
||||
self.first_motor.write_registers(0, first_motor_register_data)
|
||||
self.second_motor.write_registers(0, second_motor_register_data)
|
||||
|
||||
except Exception, error:
|
||||
print "Error occurred:", error
|
||||
self.drive_control_message = drive_control
|
||||
self.new_control_message = True
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
DriveControl()
|
||||
DriveControl()
|
||||
@@ -117,7 +117,10 @@ class IrisController(object):
|
||||
sleep(max(self.wait_time - time_diff, 0))
|
||||
|
||||
def read_registers(self):
|
||||
self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS))
|
||||
try:
|
||||
self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS))
|
||||
except Exception, error:
|
||||
print error
|
||||
|
||||
def broadcast_drive_if_current_mode(self):
|
||||
if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] < SBUS_VALUES["SBUS_MID"]:
|
||||
@@ -127,6 +130,7 @@ class IrisController(object):
|
||||
right_y_axis = self.registers[MODBUS_REGISTERS["RIGHT_STICK_Y_AXIS"]]
|
||||
|
||||
if left_y_axis == 0 and right_y_axis == 0:
|
||||
command.controller_present = False
|
||||
command.ignore_drive_control = True
|
||||
command.drive_twist.linear.x = 0
|
||||
command.drive_twist.angular.z = 0
|
||||
@@ -138,6 +142,7 @@ class IrisController(object):
|
||||
right = (right_y_axis - SBUS_VALUES["SBUS_MID"]) / SBUS_VALUES[
|
||||
"SBUS_RANGE"]
|
||||
|
||||
command.controller_present = True
|
||||
command.ignore_drive_control = \
|
||||
self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["IGNORE_CONTROL"]]] > SBUS_VALUES["SBUS_MID"]
|
||||
command.drive_twist.linear.x = (left + right) / 2.0
|
||||
|
||||
@@ -4,5 +4,5 @@
|
||||
<include file="$(find rover_main)/launch/ground_station/topic_transport_receivers.launch"/>
|
||||
|
||||
<!-- ########## Start Ground Station Interface ########## -->
|
||||
<node name="ground_station" pkg="ground_station" type="ground_station_launch.sh" output="screen"/>
|
||||
<node name="ground_station" pkg="ground_station" type="ground_station_launch.sh" required="true" output="screen"/>
|
||||
</launch>
|
||||
@@ -1,27 +1,51 @@
|
||||
<launch>
|
||||
<group ns="cameras">
|
||||
<node name="chassis_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<node name="chassis_1280x720" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17001" />
|
||||
</node>
|
||||
|
||||
<node name="undercarriage_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<node name="undercarriage_1280x720" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17002" />
|
||||
</node>
|
||||
|
||||
<node name="main_navigation_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<node name="main_navigation_1280x720" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17003" />
|
||||
</node>
|
||||
|
||||
<node name="chassis_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<node name="end_effector_1280x720" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17004" />
|
||||
</node>
|
||||
|
||||
<node name="undercarriage_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<node name="chassis_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17005" />
|
||||
</node>
|
||||
|
||||
<node name="main_navigation_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<node name="undercarriage_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17006" />
|
||||
</node>
|
||||
|
||||
<node name="main_navigation_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17007" />
|
||||
</node>
|
||||
|
||||
<node name="end_effector_640x360" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17008" />
|
||||
</node>
|
||||
|
||||
<node name="chassis_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17009" />
|
||||
</node>
|
||||
|
||||
<node name="undercarriage_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17010" />
|
||||
</node>
|
||||
|
||||
<node name="main_navigation_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17011" />
|
||||
</node>
|
||||
|
||||
<node name="end_effector_256x144" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17012" />
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
@@ -2,12 +2,25 @@
|
||||
<group ns="sender_transports">
|
||||
<arg name="target" default="192.168.1.10" />
|
||||
|
||||
<node name="udp_sender4" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<node name="ground_station_drive_udp_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17020" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="ground_station_tcp_sender" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17021" />
|
||||
<rosparam param="topics">
|
||||
[
|
||||
{name: "/cameras/chassis/camera_control", compress: true, rate: 5.0},
|
||||
{name: "/cameras/undercarriage/camera_control", compress: true, rate: 5.0},
|
||||
{name: "/cameras/main_navigation/camera_control", compress: true, rate: 5.0},
|
||||
{name: "/cameras/end_effector/camera_control", compress: true, rate: 5.0}
|
||||
]
|
||||
</rosparam>
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
@@ -14,5 +14,11 @@
|
||||
<node name="chassis" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 3" respawn="true" output="screen" >
|
||||
<param name="device_path" value="/dev/rover/camera_chassis" />
|
||||
</node>
|
||||
|
||||
<!-- Start End Effector Camera -->
|
||||
<node name="end_effector" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 4" respawn="true" output="screen" >
|
||||
<param name="is_rtsp_camera" value="True" />
|
||||
<param name="device_path" value="rtsp://192.168.1.11" />
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
@@ -2,17 +2,19 @@
|
||||
<group ns="rover_control">
|
||||
<node name="iris_controller" pkg="rover_control" type="iris_controller.py" output="screen">
|
||||
<param name="port" value="/dev/rover/ttyIRIS"/>
|
||||
<param name="hertz" value="16"/>
|
||||
<param name="hertz" value="20"/>
|
||||
</node>
|
||||
|
||||
<node name="rear_bogie" pkg="rover_control" type="drive_control.py" output="screen">
|
||||
<param name="port" value="/dev/rover/ttyBogieRear"/>
|
||||
<param name="drive_control_topic" value="drive_control/rear"/>
|
||||
<param name="drive_control_status_topic" value="drive_status/rear"/>
|
||||
</node>
|
||||
|
||||
<node name="left_bogie" pkg="rover_control" type="drive_control.py" output="screen">
|
||||
<param name="port" value="/dev/rover/ttyBogieLeft"/>
|
||||
<param name="drive_control_topic" value="drive_control/left"/>
|
||||
<param name="drive_control_status_topic" value="drive_status/left"/>
|
||||
<param name="invert_first_motor" value="True"/>
|
||||
<param name="invert_second_motor" value="True"/>
|
||||
</node>
|
||||
@@ -20,6 +22,7 @@
|
||||
<node name="right_bogie" pkg="rover_control" type="drive_control.py" output="screen">
|
||||
<param name="port" value="/dev/rover/ttyBogieRight"/>
|
||||
<param name="drive_control_topic" value="drive_control/right"/>
|
||||
<param name="drive_control_status_topic" value="drive_status/right"/>
|
||||
</node>
|
||||
|
||||
<node name="drive_coordinator" pkg="rover_control" type="drive_coordinator.py" output="screen"/>
|
||||
|
||||
@@ -3,5 +3,9 @@
|
||||
<node name="ground_station_drive_command" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||
<param name="port" value="17020" />
|
||||
</node>
|
||||
|
||||
<node name="ground_station_tcp_topics" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
|
||||
<param name="port" value="17021" />
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
@@ -2,51 +2,99 @@
|
||||
<group ns="sender_transports">
|
||||
<arg name="target" default="192.168.1.15" />
|
||||
|
||||
<node name="chassis_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<node name="chassis_1280x720" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17001" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/chassis/image_640x360/compressed", compress: false, rate: 30.0}]
|
||||
[{name: "/cameras/chassis/image_1280x720/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="undercarriage_1280x720" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17002" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/undercarriage/image_1280x720/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="main_navigation_1280x720" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17003" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/main_navigation/image_1280x720/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="end_effector_1280x720" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17004" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/end_effector/image_1280x720/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="chassis_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17005" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/chassis/image_640x360/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="undercarriage_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17002" />
|
||||
<param name="destination_port" value="17006" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/undercarriage/image_640x360/compressed", compress: false, rate: 30.0}]
|
||||
[{name: "/cameras/undercarriage/image_640x360/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="main_navigation_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17003" />
|
||||
<param name="destination_port" value="17007" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/main_navigation/image_640x360/compressed", compress: false, rate: 30.0}]
|
||||
[{name: "/cameras/main_navigation/image_640x360/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="end_effector_640x360" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17008" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/end_effector/image_640x360/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="chassis_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17004" />
|
||||
<param name="destination_port" value="17009" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/chassis/image_256x144/compressed", compress: false, rate: 30.0}]
|
||||
[{name: "/cameras/chassis/image_256x144/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="undercarriage_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17005" />
|
||||
<param name="destination_port" value="17010" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/undercarriage/image_256x144/compressed", compress: false, rate: 30.0}]
|
||||
[{name: "/cameras/undercarriage/image_256x144/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="main_navigation_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17006" />
|
||||
<param name="destination_port" value="17011" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/main_navigation/image_256x144/compressed", compress: false, rate: 30.0}]
|
||||
[{name: "/cameras/main_navigation/image_256x144/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<node name="end_effector_256x144" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
|
||||
<param name="destination_addr" value="$(arg target)" />
|
||||
<param name="destination_port" value="17012" />
|
||||
<rosparam param="topics">
|
||||
[{name: "/cameras/end_effector/image_256x144/compressed", compress: false, rate: 0.0}]
|
||||
</rosparam>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
211
software/ros_packages/rover_status/CMakeLists.txt
Normal file
211
software/ros_packages/rover_status/CMakeLists.txt
Normal file
@@ -0,0 +1,211 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(rover_status)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a run_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
add_message_files(
|
||||
FILES
|
||||
RoverSysStatus.msg
|
||||
BogieStatuses.msg
|
||||
FrSkyStatus.msg
|
||||
JetsonInfo.msg
|
||||
CameraStatuses.msg
|
||||
GPSInfo.msg
|
||||
MiscStatuses.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
)
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES system_statuses
|
||||
# CATKIN_DEPENDS roscpp rospy std_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
# include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/system_statuses.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/system_statuses_node.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_system_statuses.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
Binary file not shown.
3
software/ros_packages/rover_status/msg/BogieStatuses.msg
Normal file
3
software/ros_packages/rover_status/msg/BogieStatuses.msg
Normal file
@@ -0,0 +1,3 @@
|
||||
int8 bogie_connection_1
|
||||
int8 bogie_connection_2
|
||||
int8 bogie_connection_3
|
||||
@@ -0,0 +1,4 @@
|
||||
int8 camera_zed
|
||||
int8 camera_undercarriage
|
||||
int8 camera_chassis
|
||||
int8 camera_main_navigation
|
||||
1
software/ros_packages/rover_status/msg/FrSkyStatus.msg
Normal file
1
software/ros_packages/rover_status/msg/FrSkyStatus.msg
Normal file
@@ -0,0 +1 @@
|
||||
int8 FrSky_controller_connection_status
|
||||
2
software/ros_packages/rover_status/msg/GPSInfo.msg
Normal file
2
software/ros_packages/rover_status/msg/GPSInfo.msg
Normal file
@@ -0,0 +1,2 @@
|
||||
int8 UTC_GPS_time
|
||||
int8 GPS_connection_status
|
||||
5
software/ros_packages/rover_status/msg/JetsonInfo.msg
Normal file
5
software/ros_packages/rover_status/msg/JetsonInfo.msg
Normal file
@@ -0,0 +1,5 @@
|
||||
float64 jetson_CPU
|
||||
float64 jetson_RAM
|
||||
float64 jetson_EMMC
|
||||
float64 jetson_NVME_SSD
|
||||
float64 jetson_GPU_temp
|
||||
5
software/ros_packages/rover_status/msg/MiscStatuses.msg
Normal file
5
software/ros_packages/rover_status/msg/MiscStatuses.msg
Normal file
@@ -0,0 +1,5 @@
|
||||
int8 arm_connection_status
|
||||
int8 arm_end_effector_connection_statuses
|
||||
int8 sample_containment_connection_status
|
||||
int8 tower_connection_status
|
||||
int8 chassis_pan_tilt_connection_status
|
||||
19
software/ros_packages/rover_status/msg/RoverSysStatus.msg
Normal file
19
software/ros_packages/rover_status/msg/RoverSysStatus.msg
Normal file
@@ -0,0 +1,19 @@
|
||||
int8 UTC_GPS_time
|
||||
int8 bogie_connection_1
|
||||
int8 bogie_connection_2
|
||||
int8 bogie_connection_3
|
||||
int8 arm_connection_status
|
||||
int8 arm_end_effector_connection_statuses
|
||||
int8 camera_zed
|
||||
int8 camera_undercarriage
|
||||
int8 camera_chassis
|
||||
int8 camera_main_navigation
|
||||
int8 sample_containment_connection_status
|
||||
int8 tower_connection_status
|
||||
int8 chassis_pan_tilt_connection_status
|
||||
int8 GPS_connection_status
|
||||
int8 jetson_CPU
|
||||
int8 jetson_RAM
|
||||
int8 jetson_EMMC
|
||||
int8 jetson_NVME_SSD
|
||||
int8 FrSky_controller_connection_status
|
||||
68
software/ros_packages/rover_status/package.xml
Normal file
68
software/ros_packages/rover_status/package.xml
Normal file
@@ -0,0 +1,68 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>rover_status</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The rover_status package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="matcurtay@matcurtay.net">matcurtay</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>BSD</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/rover_status</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<build_depend>message_generation</build_depend>
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
26
software/ros_packages/rover_status/src/camera_2_updater.py
Executable file
26
software/ros_packages/rover_status/src/camera_2_updater.py
Executable file
@@ -0,0 +1,26 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# import rospy
|
||||
# from system_statuses.msg import Camera2Changer
|
||||
|
||||
|
||||
# def camera_2_changer():
|
||||
# pub = rospy.Publisher('camera_2_changer_chatter', Camera2Changer, queue_size=10)
|
||||
# rospy.init_node('camera_2_changer_talker', anonymous=True)
|
||||
# # r = rospy.sleep(10) # 10hz
|
||||
# msg = Camera2Changer()
|
||||
# msg.camera_2_value = 0
|
||||
|
||||
# while not rospy.is_shutdown():
|
||||
# msg.camera_2_value = (msg.camera_2_value + 1) % 2
|
||||
# rospy.loginfo(msg)
|
||||
# pub.publish(msg)
|
||||
# r.sleep()
|
||||
## rospy.sleep(2.)
|
||||
|
||||
|
||||
#if __name__ == '__main__':
|
||||
# try:
|
||||
# camera_2_changer()
|
||||
# except rospy.ROSInterruptException:
|
||||
# pass
|
||||
80
software/ros_packages/rover_status/src/rover_statuses.py
Executable file
80
software/ros_packages/rover_status/src/rover_statuses.py
Executable file
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
from system_statuses.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
|
||||
|
||||
|
||||
# THIS IS A SUPER ROUGH EXAMPLE OF HOW TO PULL THE DATA
|
||||
# You can create your own message formats in the msg folder
|
||||
# This is a simple example of pulling data from system_statuses_node.py
|
||||
# and storing them into self values.
|
||||
# The ground control code sounds like it'll be fairly different in format.
|
||||
|
||||
class RoverStatuses:
|
||||
|
||||
def __init__(self):
|
||||
|
||||
rospy.init_node('RoverStatuses')
|
||||
|
||||
# self.pub = rospy.Publisher('rover_statuses_chatter', RoverSysStatus, queue_size=10)
|
||||
|
||||
# Subscription examples on pulling data from system_statuses_node.py
|
||||
rospy.Subscriber('camera_system_status_chatter', CameraStatuses, self.__camera_callback)
|
||||
rospy.Subscriber('bogie_system_status_chatter', BogieStatuses, self.__bogie_callback)
|
||||
rospy.Subscriber('FrSky_system_status_chatter', FrSkyStatus, self.__frsky_callback)
|
||||
rospy.Subscriber('GPS_system_status_chatter', GPSInfo, self.__gps_callback)
|
||||
rospy.Subscriber('jetson_system_status_chatter', JetsonInfo, self.__jetson_callback)
|
||||
rospy.Subscriber('misc_system_status_chatter', MiscStatuses, self.__misc_callback)
|
||||
|
||||
self.camera_msg = CameraStatuses()
|
||||
self.bogie_msg = BogieStatuses()
|
||||
self.FrSky_msg = FrSkyStatus()
|
||||
self.GPS_msg = GPSInfo()
|
||||
self.jetson_msg = JetsonInfo()
|
||||
self.misc_msg = MiscStatuses()
|
||||
|
||||
def __camera_callback(self, data):
|
||||
self.camera_msg.camera_zed = data.camera_zed
|
||||
self.camera_msg.camera_undercarriage = data.camera_undercarriage
|
||||
self.camera_msg.camera_chassis = data.camera_chassis
|
||||
self.camera_msg.camera_main_navigation = data.camera_main_navigation
|
||||
|
||||
def __frsky_callback(self, data):
|
||||
self.FrSky_msg.FrSky_controller_connection_status = data.FrSky_controller_connection_status
|
||||
|
||||
def __bogie_callback(self, data):
|
||||
self.bogie_msg.bogie_connection_1 = data.bogie_connection_1
|
||||
self.bogie_msg.bogie_connection_2 = data.bogie_connection_2
|
||||
self.bogie_msg.bogie_connection_3 = data.bogie_connection_3
|
||||
|
||||
def __jetson_callback(self, data):
|
||||
self.jetson_msg.jetson_CPU = data.jetson_CPU
|
||||
self.jetson_msg.jetson_RAM = data.jetson_RAM
|
||||
self.jetson_msg.jetson_EMMC = data.jetson_EMMC
|
||||
self.jetson_msg.jetson_NVME_SSD = data.jetson_NVME_SSD
|
||||
self.jetson_msg.jetson_GPU_temp = data.jetson_GPU_temp
|
||||
rospy.loginfo(self.jetson_msg)
|
||||
|
||||
def __gps_callback(self, data):
|
||||
self.GPS_msg.UTC_GPS_time = data.UTC_GPS_time
|
||||
self.GPS_msg.GPS_connection_status = data.GPS_connection_status
|
||||
|
||||
def __misc_callback(self, data):
|
||||
self.misc_msg.arm_connection_status = data.arm_connection_status
|
||||
self.misc_msg.arm_end_effector_connection_statuses = data.arm_end_effector_connection_statuses
|
||||
self.misc_msg.sample_containment_connection_status = data.sample_containment_connection_status
|
||||
self.misc_msg.tower_connection_status = data.tower_connection_status
|
||||
self.misc_msg.chassis_pan_tilt_connection_status = data.chassis_pan_tilt_connection_status
|
||||
|
||||
def run(self):
|
||||
rospy.Subscriber('camera_system_status_chatter', CameraStatuses, self.__camera_callback)
|
||||
rospy.Subscriber('bogie_system_status_chatter', BogieStatuses, self.__bogie_callback)
|
||||
rospy.Subscriber('FrSky_system_status_chatter', FrSkyStatus, self.__frsky_callback)
|
||||
rospy.Subscriber('GPS_system_status_chatter', GPSInfo, self.__gps_callback)
|
||||
rospy.Subscriber('jetson_system_status_chatter', JetsonInfo, self.__jetson_callback)
|
||||
rospy.Subscriber('misc_system_status_chatter', MiscStatuses, self.__misc_callback)
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
rover_statuses = RoverStatuses()
|
||||
rover_statuses.run()
|
||||
247
software/ros_packages/rover_status/src/system_statuses_node.py
Executable file
247
software/ros_packages/rover_status/src/system_statuses_node.py
Executable file
@@ -0,0 +1,247 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
import os.path
|
||||
import psutil
|
||||
import subprocess
|
||||
from system_statuses.msg import CameraStatuses, BogieStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
|
||||
from rover_control.msg import DriveCommandMessage
|
||||
|
||||
class SystemStatuses:
|
||||
|
||||
def __init__(self):
|
||||
# Camera path locations
|
||||
self.system_path_locations = [
|
||||
'/dev/rover/camera_zed',
|
||||
'/dev/rover/camera_undercarriage',
|
||||
'/dev/rover/camera_chassis',
|
||||
'/dev/rover/camera_main_navigation'
|
||||
]
|
||||
|
||||
# filesystem paths for EMMC [0] and NVME_SSD [1]
|
||||
# -- UPDATE [1] FOR JETSON --
|
||||
self.file_systems_EMMC_NVMe_SSD = [
|
||||
'/',
|
||||
'/dev/shm'
|
||||
]
|
||||
|
||||
rospy.init_node('SystemStatuses')
|
||||
|
||||
# init all publisher functions
|
||||
self.pub_camera = rospy.Publisher('camera_system_status_chatter', CameraStatuses, queue_size=10)
|
||||
self.pub_bogie = rospy.Publisher('bogie_system_status_chatter', BogieStatuses, queue_size=10)
|
||||
self.pub_FrSky = rospy.Publisher('FrSky_system_status_chatter', FrSkyStatus, queue_size=10)
|
||||
self.pub_GPS = rospy.Publisher('GPS_system_status_chatter', GPSInfo, queue_size=10)
|
||||
self.pub_jetson = rospy.Publisher('jetson_system_status_chatter', JetsonInfo, queue_size=10)
|
||||
self.pub_Misc = rospy.Publisher('misc_system_status_chatter', MiscStatuses, queue_size=10)
|
||||
|
||||
# init all message variables
|
||||
self.camera_msg = CameraStatuses()
|
||||
self.bogie_msg = BogieStatuses()
|
||||
self.FrSky_msg = FrSkyStatus()
|
||||
self.GPS_msg = GPSInfo()
|
||||
self.jetson_msg = JetsonInfo()
|
||||
self.misc_msg = MiscStatuses()
|
||||
|
||||
# init all message values
|
||||
self.__pull_new_message_values()
|
||||
|
||||
# init all previous values
|
||||
self.__update_all_previous_values()
|
||||
|
||||
# init all RoverSysMessage values
|
||||
def __pull_new_message_values(self):
|
||||
self.__set_gps_info()
|
||||
self.__set_bogie_connection_statuses()
|
||||
self.__set_arm_connection_status()
|
||||
self.__set_arm_end_effector_connection_statuses()
|
||||
self.__set_cameras()
|
||||
self.__set_sample_containment_connection_status()
|
||||
self.__set_tower_connection_status()
|
||||
self.__set_chassis_pan_tilt_connection_status()
|
||||
self.__set_jetson_usage_information()
|
||||
self.__set_frsky_controller_connection_status()
|
||||
|
||||
# Pulls the UTC GPS Time (WIP)
|
||||
def __set_gps_info(self):
|
||||
self.GPS_msg.UTC_GPS_time = 0
|
||||
self.GPS_msg.GPS_connection_status = 0
|
||||
|
||||
# Pulls bogie connection statuses (WIP)
|
||||
def __set_bogie_connection_statuses(self):
|
||||
self.bogie_msg.bogie_connection_1 = 0
|
||||
self.bogie_msg.bogie_connection_2 = 0
|
||||
self.bogie_msg.bogie_connection_3 = 0
|
||||
|
||||
# Checks arm connection status (WIP)
|
||||
def __set_arm_connection_status(self):
|
||||
self.misc_msg.arm_connection_status = 0
|
||||
|
||||
# Checks Arm End Effector Connection Statuses (WIP)
|
||||
def __set_arm_end_effector_connection_statuses(self):
|
||||
self.misc_msg.arm_end_effector_connection_statuses = 0
|
||||
|
||||
# Sets the Camera values (zed, undercarriage, chassis, and main_nav
|
||||
def __set_cameras(self):
|
||||
# Check if camera_zed is found
|
||||
self.camera_msg.camera_zed = 1 if os.path.exists(self.system_path_locations[0]) else 0
|
||||
# Check if camera_undercarriage is found
|
||||
self.camera_msg.camera_undercarriage = 1 if os.path.exists(self.system_path_locations[1]) else 0
|
||||
# Check if camera_chassis is found
|
||||
self.camera_msg.camera_chassis = 1 if os.path.exists(self.system_path_locations[2]) else 0
|
||||
# Check if camera_main_navigation is found
|
||||
self.camera_msg.camera_main_navigation = 1 if os.path.exists(self.system_path_locations[3]) else 0
|
||||
|
||||
# Checks Sample Containment Connection Status (WIP)
|
||||
def __set_sample_containment_connection_status(self):
|
||||
self.misc_msg.sample_containment_connection_status = 0
|
||||
|
||||
def __set_tower_connection_status(self):
|
||||
self.misc_msg.tower_connection_status = 0
|
||||
|
||||
# Checks Tower Connection Status (WIP)
|
||||
def __set_chassis_pan_tilt_connection_status(self):
|
||||
self.misc_msg.chassis_pan_tilt_connection_status = 0
|
||||
|
||||
# Get Jetson Statuses (WIP)
|
||||
def __set_jetson_usage_information(self):
|
||||
self.jetson_msg.jetson_CPU = psutil.cpu_percent()
|
||||
mem = psutil.virtual_memory()
|
||||
self.jetson_msg.jetson_RAM = mem.percent
|
||||
self.jetson_msg.jetson_EMMC = self.__used_percent_fs(self.file_systems_EMMC_NVMe_SSD[0])
|
||||
self.jetson_msg.jetson_NVME_SSD = self.__used_percent_fs(self.file_systems_EMMC_NVMe_SSD[1])
|
||||
|
||||
# Temperature
|
||||
# This try except causes a bunch of annoying messages, but lets it run on non-jetson devices
|
||||
# sets to -1.0 if sensor fails to give it a default value notifying failure to pull
|
||||
try:
|
||||
sensor_temperatures = subprocess.check_output("sensors | grep temp", shell=True)
|
||||
parsed_temps = sensor_temperatures.replace("\xc2\xb0C","").replace("(crit = ","").replace("temp1:","")\
|
||||
.replace("\n","").replace("+","").split()
|
||||
self.jetson_msg.jetson_GPU_temp = parsed_temps[4]
|
||||
except subprocess.CalledProcessError:
|
||||
print 'sensors call failed (potential reason: on VM)'
|
||||
self.jetson_msg.jetson_GPU_temp = -1.0
|
||||
|
||||
# EMMC and NVMe_SSD used % calculation
|
||||
def __used_percent_fs(self, pathname):
|
||||
statvfs = os.statvfs(pathname)
|
||||
# percentage :: USED:
|
||||
# used amount: blocks - bfree
|
||||
# used%: used_amount / (used_amount + bavail)
|
||||
used_available = (statvfs.f_frsize * statvfs.f_blocks / 1024) - (statvfs.f_frsize * statvfs.f_bfree / 1024.0)
|
||||
used_percent = used_available / (used_available + (statvfs.f_frsize * statvfs.f_bavail / 1024.0))
|
||||
# Round 4 for 2 decimal accuracy
|
||||
value = 100 * round(used_percent, 4)
|
||||
return value
|
||||
|
||||
# Check FrSky Controller Connection Status (WIP)
|
||||
def __set_frsky_controller_connection_status(self):
|
||||
rospy.Subscriber('/rover_control/command_control/iris_drive', DriveCommandMessage, self.__frsky_callback)
|
||||
|
||||
def __frsky_callback(self, data):
|
||||
self.FrSky_msg.FrSky_controller_connection_status = data.controller_present
|
||||
|
||||
# Used mainly for init, sets all previous values in one go
|
||||
def __update_all_previous_values(self):
|
||||
self.__set_previous_camera_values()
|
||||
self.__set_previous_jetson_values()
|
||||
self.__set_previous_frsky_value()
|
||||
self.__set_previous_bogie_values()
|
||||
self.__set_previous_gps_values()
|
||||
self.__set_previous_misc_values()
|
||||
|
||||
def __set_previous_camera_values(self):
|
||||
self.previous_camera_zed = self.camera_msg.camera_zed
|
||||
self.previous_camera_undercarriage = self.camera_msg.camera_undercarriage
|
||||
self.previous_camera_chassis = self.camera_msg.camera_chassis
|
||||
self.previous_camera_main_navigation = self.camera_msg.camera_main_navigation
|
||||
|
||||
def __set_previous_jetson_values(self):
|
||||
self.previous_jetson_CPU = self.jetson_msg.jetson_CPU
|
||||
self.previous_jetson_RAM = self.jetson_msg.jetson_RAM
|
||||
self.previous_jetson_EMMC = self.jetson_msg.jetson_EMMC
|
||||
self.previous_jetson_NVME_SSD = self.jetson_msg.jetson_NVME_SSD
|
||||
self.previous_jetson_GPU_temp = self.jetson_msg.jetson_GPU_temp
|
||||
|
||||
def __set_previous_frsky_value(self):
|
||||
self.previous_FrSky_controller_connection_status = self.FrSky_msg.FrSky_controller_connection_status
|
||||
|
||||
def __set_previous_bogie_values(self):
|
||||
self.previous_bogie_connection_1 = self.bogie_msg.bogie_connection_1
|
||||
self.previous_bogie_connection_2 = self.bogie_msg.bogie_connection_2
|
||||
self.previous_bogie_connection_3 = self.bogie_msg.bogie_connection_3
|
||||
|
||||
def __set_previous_gps_values(self):
|
||||
self.previous_UTC_GPS_time = self.GPS_msg.UTC_GPS_time
|
||||
self.previous_GPS_connection_status = self.GPS_msg.GPS_connection_status
|
||||
|
||||
def __set_previous_misc_values(self):
|
||||
self.previous_arm_connection_status = self.misc_msg.arm_connection_status
|
||||
self.previous_arm_end_effector_connection_statuses = self.misc_msg.arm_end_effector_connection_statuses
|
||||
self.previous_chassis_pan_tilt_connection_status = self.misc_msg.chassis_pan_tilt_connection_status
|
||||
self.previous_sample_containment_connection_status = self.misc_msg.sample_containment_connection_status
|
||||
self.previous_tower_connection_status = self.misc_msg.tower_connection_status
|
||||
|
||||
def run(self):
|
||||
r = rospy.Rate(10) # 10Hz
|
||||
while not rospy.is_shutdown():
|
||||
# Update all message values
|
||||
self.__pull_new_message_values()
|
||||
|
||||
# Camera Check -- if current value is now different from previous value,
|
||||
# update the previous value for cameras and publish to listening Subscribers
|
||||
if (self.camera_msg.camera_zed != self.previous_camera_zed or
|
||||
self.camera_msg.camera_undercarriage != self.previous_camera_undercarriage or
|
||||
self.camera_msg.camera_chassis != self.previous_camera_chassis or
|
||||
self.camera_msg.camera_main_navigation != self.previous_camera_main_navigation):
|
||||
self.__set_previous_camera_values()
|
||||
self.pub_camera.publish(self.camera_msg)
|
||||
|
||||
# Placeholder Jetson Info Check
|
||||
if (self.jetson_msg.jetson_CPU != self.previous_jetson_CPU or
|
||||
self.jetson_msg.jetson_RAM != self.previous_jetson_RAM or
|
||||
self.jetson_msg.jetson_EMMC != self.previous_jetson_EMMC or
|
||||
self.jetson_msg.jetson_NVME_SSD != self.previous_jetson_NVME_SSD or
|
||||
self.jetson_msg.jetson_GPU_temp != self.previous_jetson_GPU_temp):
|
||||
self.__set_previous_jetson_values()
|
||||
self.pub_jetson.publish(self.jetson_msg)
|
||||
|
||||
# Placeholder FrSky Controller Check
|
||||
if self.FrSky_msg.FrSky_controller_connection_status != self.previous_FrSky_controller_connection_status:
|
||||
self.__set_previous_frsky_value()
|
||||
self.pub_FrSky.publish(self.FrSky_msg)
|
||||
|
||||
# Placeholder bogie status check
|
||||
if (self.bogie_msg.bogie_connection_1 != self.previous_bogie_connection_1 or
|
||||
self.bogie_msg.bogie_connection_2 != self.previous_bogie_connection_2 or
|
||||
self.bogie_msg.bogie_connection_3 != self.previous_bogie_connection_3):
|
||||
self.__set_previous_bogie_values()
|
||||
self.pub_bogie.publish(self.bogie_msg)
|
||||
|
||||
# Placeholder GPS Information check
|
||||
if (self.GPS_msg.UTC_GPS_time != self.previous_UTC_GPS_time or
|
||||
self.GPS_msg.UTC_GPS_time != self.previous_GPS_connection_status):
|
||||
self.__set_previous_gps_values()
|
||||
self.pub_GPS.publish(self.GPS_msg)
|
||||
|
||||
# Placeholder Misc Information check
|
||||
if (self.misc_msg.arm_connection_status !=
|
||||
self.previous_arm_connection_status or
|
||||
self.misc_msg.arm_end_effector_connection_statuses !=
|
||||
self.previous_arm_end_effector_connection_statuses or
|
||||
self.misc_msg.chassis_pan_tilt_connection_status !=
|
||||
self.previous_chassis_pan_tilt_connection_status or
|
||||
self.misc_msg.sample_containment_connection_status !=
|
||||
self.previous_sample_containment_connection_status or
|
||||
self.misc_msg.tower_connection_status !=
|
||||
self.previous_tower_connection_status):
|
||||
self.__set_previous_misc_values()
|
||||
self.pub_Misc.publish(self.misc_msg)
|
||||
|
||||
# rospy.loginfo(self.msg)
|
||||
r.sleep()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
system_status = SystemStatuses()
|
||||
system_status.run()
|
||||
@@ -9,6 +9,7 @@
|
||||
folders_to_link=(
|
||||
rover_camera
|
||||
rover_control
|
||||
rover_status
|
||||
zed_wrapper
|
||||
nimbro_topic_transport
|
||||
rover_main
|
||||
|
||||
Reference in New Issue
Block a user