mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 18:21:15 +00:00
Merge branch 'master' of https://github.com/OSURoboticsClub/Rover_2017_2018
This commit is contained in:
Chris Pham
197
software/ros_packages/ground_station/CMakeLists.txt
Normal file
197
software/ros_packages/ground_station/CMakeLists.txt
Normal file
@@ -0,0 +1,197 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(ground_station)
|
||||
|
||||
## 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
|
||||
rospy
|
||||
)
|
||||
|
||||
## 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
|
||||
# )
|
||||
|
||||
## 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 # Or other packages containing 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 ground_station
|
||||
# CATKIN_DEPENDS rospy
|
||||
# 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}/ground_station.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/ground_station_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_ground_station.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)
|
||||
62
software/ros_packages/ground_station/package.xml
Normal file
62
software/ros_packages/ground_station/package.xml
Normal file
@@ -0,0 +1,62 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>ground_station</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The ground_station package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="caperren@todo.todo">caperren</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>TODO</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/ground_station</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>rospy</build_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
17
software/ros_packages/ground_station/scripts/ground_station_launch.sh
Executable file
17
software/ros_packages/ground_station/scripts/ground_station_launch.sh
Executable file
@@ -0,0 +1,17 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
current_folder_name="scripts"
|
||||
current_folder_name_length=`expr length $current_folder_name`
|
||||
|
||||
launch_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
|
||||
launch_dir_length=`expr length $launch_dir`
|
||||
|
||||
launch_dir_length_without_current_folder=$(($launch_dir_length-$current_folder_name_length))
|
||||
|
||||
script_launch_path="${launch_dir:0:$launch_dir_length_without_current_folder}/src"
|
||||
cd $script_launch_path
|
||||
|
||||
sleep 5
|
||||
|
||||
export DISPLAY=:0
|
||||
python ground_station.py
|
||||
@@ -0,0 +1,187 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtWidgets
|
||||
import logging
|
||||
from inputs import devices, GamePad
|
||||
from time import time
|
||||
|
||||
import rospy
|
||||
from rover_control.msg import DriveCommandMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro"
|
||||
|
||||
DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive"
|
||||
|
||||
DRIVE_COMMAND_HERTZ = 15
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
#####################################
|
||||
class LogitechJoystick(QtCore.QThread):
|
||||
def __init__(self):
|
||||
super(LogitechJoystick, self).__init__()
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
self.setup_controller_flag = True
|
||||
self.data_acquisition_and_broadcast_flag = True
|
||||
self.controller_acquired = False
|
||||
|
||||
# ########## Class Variables ##########
|
||||
self.gamepad = None # type: GamePad
|
||||
|
||||
self.controller_states = {
|
||||
"left_stick_x_axis": 0,
|
||||
"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,
|
||||
|
||||
"dpad_x": 0,
|
||||
"dpad_y": 0,
|
||||
|
||||
"select_pressed": 0,
|
||||
"start_pressed": 0,
|
||||
"home_pressed": 0,
|
||||
|
||||
"a_pressed": 0,
|
||||
"b_pressed": 0,
|
||||
"x_pressed": 0,
|
||||
"y_pressed": 0
|
||||
}
|
||||
|
||||
self.raw_mapping_to_class_mapping = {
|
||||
"ABS_X": "left_stick_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_HAT0X": "dpad_x",
|
||||
"ABS_HAT0Y": "dpad_y",
|
||||
|
||||
"BTN_SELECT": "select_pressed",
|
||||
"BTN_START": "start_pressed",
|
||||
"BTN_MODE": "home_pressed",
|
||||
|
||||
"BTN_SOUTH": "a_pressed",
|
||||
"BTN_EAST": "b_pressed",
|
||||
"BTN_NORTH": "x_pressed",
|
||||
"BTN_WEST": "y_pressed"
|
||||
}
|
||||
|
||||
self.ready = False
|
||||
|
||||
self.start()
|
||||
|
||||
def run(self):
|
||||
|
||||
while self.run_thread_flag:
|
||||
if self.setup_controller_flag:
|
||||
self.controller_acquired = self.__setup_controller()
|
||||
self.setup_controller_flag = False
|
||||
if self.data_acquisition_and_broadcast_flag:
|
||||
self.__get_controller_data()
|
||||
|
||||
def __setup_controller(self):
|
||||
for device in devices.gamepads:
|
||||
print device
|
||||
if device.name == GAME_CONTROLLER_NAME:
|
||||
self.gamepad = device
|
||||
|
||||
return True
|
||||
return False
|
||||
|
||||
def __get_controller_data(self):
|
||||
if self.controller_acquired:
|
||||
events = self.gamepad.read()
|
||||
|
||||
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):
|
||||
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
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
|
||||
# ########## Get the Pick And Plate instance of the logger ##########
|
||||
self.logger = logging.getLogger("groundstation")
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
self.joystick = LogitechJoystick()
|
||||
|
||||
# ########## Class Variables ##########
|
||||
# Publishers
|
||||
self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1)
|
||||
|
||||
self.wait_time = 1 / DRIVE_COMMAND_HERTZ
|
||||
|
||||
def run(self):
|
||||
while self.run_thread_flag:
|
||||
|
||||
start_time = time()
|
||||
|
||||
self.__update_and_publish()
|
||||
|
||||
time_diff = time() - start_time
|
||||
|
||||
self.msleep(max(self.wait_time - time_diff, 0))
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
def __update_and_publish(self):
|
||||
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"]
|
||||
|
||||
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 on_kill_threads_requested__slot(self):
|
||||
self.run_thread_flag = False
|
||||
@@ -0,0 +1,127 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore
|
||||
from os import makedirs, rename, walk, unlink
|
||||
from os.path import exists, getmtime
|
||||
from os import environ
|
||||
import logging
|
||||
from datetime import datetime
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
MAX_NUM_LOG_FILES = 30
|
||||
|
||||
|
||||
#####################################
|
||||
# Logger Definition
|
||||
#####################################
|
||||
class Logger(QtCore.QObject):
|
||||
def __init__(self, console_output=True):
|
||||
super(Logger, self).__init__()
|
||||
|
||||
# ########## Local class variables ##########
|
||||
self.console_output = console_output
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
|
||||
# # ########## Get the Pick And Plate instance of the logger ##########
|
||||
self.logger = logging.getLogger("groundstation")
|
||||
|
||||
# ########## Set variables with useful paths ##########
|
||||
self.appdata_base_directory = environ["HOME"] + "/.groundstation"
|
||||
self.log_directory = self.appdata_base_directory + "//logs"
|
||||
self.log_file_path = self.log_directory + "//log.txt"
|
||||
|
||||
# ########## Cleanup old log files ##########
|
||||
self.__cleanup_log_files()
|
||||
|
||||
# ########## Set up logger with desired settings ##########
|
||||
self.__setup_logger()
|
||||
|
||||
# ########## Place divider in log file to see new program launch ##########
|
||||
self.__add_startup_log_buffer_text()
|
||||
|
||||
def __setup_logger(self):
|
||||
# Get the appdata directory and make the log path if it doesn't exist
|
||||
if not exists(self.log_directory):
|
||||
makedirs(self.log_directory)
|
||||
|
||||
# Set the debugging level
|
||||
self.logger.setLevel(logging.DEBUG)
|
||||
|
||||
# Make a formatter with the log line format wanted
|
||||
formatter = logging.Formatter(fmt='%(levelname)s : %(asctime)s : %(message)s', datefmt='%m/%d/%y %H:%M:%S')
|
||||
|
||||
# Set up a file handler so everything can be saved and attach it to the logger
|
||||
file_handler = logging.FileHandler(filename=self.log_file_path)
|
||||
file_handler.setFormatter(formatter)
|
||||
file_handler.setLevel(logging.DEBUG)
|
||||
self.logger.addHandler(file_handler)
|
||||
|
||||
# Enable console output if requested
|
||||
if self.console_output:
|
||||
console_handler = logging.StreamHandler()
|
||||
console_handler.setFormatter(formatter)
|
||||
console_handler.setLevel(logging.DEBUG)
|
||||
self.logger.addHandler(console_handler)
|
||||
|
||||
def __cleanup_log_files(self):
|
||||
# This copies the existing log.txt file to an old version with a datetime stamp
|
||||
# It then checks if there are too many log files, and if so, deletes the oldest
|
||||
if exists(self.log_directory):
|
||||
# Get the number of log files
|
||||
num_log_files = self.__get_num_files_in_directory(self.log_directory)
|
||||
|
||||
# Check that we actually have log files
|
||||
if num_log_files > 0:
|
||||
date_time = datetime.now().strftime("%Y%m%d-%H%M%S")
|
||||
|
||||
# If we do, move the current logfile to a backup in the format old_log_datetime
|
||||
if exists(self.log_file_path):
|
||||
rename(self.log_file_path, self.log_directory + "\\old_log_" + date_time + ".txt")
|
||||
|
||||
# If we have more than the max log files delete the oldest one
|
||||
if num_log_files >= MAX_NUM_LOG_FILES:
|
||||
unlink(self.__get_name_of_oldest_file(self.log_directory))
|
||||
|
||||
def __add_startup_log_buffer_text(self):
|
||||
# Prints a header saying when the program started
|
||||
self.logger.info("########## Application Starting ##########")
|
||||
|
||||
@staticmethod
|
||||
def __get_name_of_oldest_file(input_path):
|
||||
oldest_file_path = None
|
||||
previous_oldest_time = 0
|
||||
|
||||
# Walk the directory passed in to get all folders and files
|
||||
for dir_path, dir_names, file_names in walk(input_path):
|
||||
# Go through all of the filenames found
|
||||
for file in file_names:
|
||||
# Recreate the full path and get the modified time of the file
|
||||
current_path = dir_path + "\\" + file
|
||||
time = getmtime(current_path)
|
||||
|
||||
# Default case for if the variables above have not been initially set
|
||||
if previous_oldest_time == 0:
|
||||
previous_oldest_time = time
|
||||
oldest_file_path = current_path
|
||||
|
||||
# Saves the oldest time and file path of the current file if it's older (lower timestamp) than the
|
||||
# last file saved in the variables
|
||||
if time < previous_oldest_time:
|
||||
previous_oldest_time = time
|
||||
oldest_file_path = current_path
|
||||
|
||||
# Returns the path to the oldest file after checking all the files
|
||||
return oldest_file_path
|
||||
|
||||
@staticmethod
|
||||
def __get_num_files_in_directory(input_path):
|
||||
# Walk the directory passed in to get all the files
|
||||
for _, _, file_names in walk(input_path):
|
||||
# Return the number of files found in the directory
|
||||
return len(file_names)
|
||||
@@ -0,0 +1,426 @@
|
||||
'''
|
||||
Mapping.py: Objected Orientated Google Maps for Python
|
||||
ReWritten by Chris Pham
|
||||
|
||||
Copyright OSURC, orginal code from GooMPy by Alec Singer and Simon D. Levy
|
||||
|
||||
This code is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as
|
||||
published by the Free Software Foundation, either version 3 of the
|
||||
License, or (at your option) any later version.
|
||||
This code is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
You should have received a copy of the GNU Lesser General Public License
|
||||
along with this code. If not, see <http://www.gnu.org/licenses/>.
|
||||
'''
|
||||
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
import math
|
||||
import urllib2
|
||||
from io import StringIO, BytesIO
|
||||
import os
|
||||
import time
|
||||
import PIL.ImageDraw
|
||||
import signing
|
||||
import RoverMapHelper as MapHelper
|
||||
|
||||
#####################################
|
||||
# Constants
|
||||
#####################################
|
||||
_KEYS = []
|
||||
# Number of pixels in half the earth's circumference at zoom = 21
|
||||
_EARTHPIX = 268435456
|
||||
# Number of decimal places for rounding coordinates
|
||||
_DEGREE_PRECISION = 4
|
||||
# Larget tile we can grab without paying
|
||||
_TILESIZE = 640
|
||||
# Fastest rate at which we can download tiles without paying
|
||||
_GRABRATE = 4
|
||||
# Pixel Radius of Earth for calculations
|
||||
_PIXRAD = _EARTHPIX / math.pi
|
||||
_DISPLAYPIX = _EARTHPIX / 2000
|
||||
|
||||
file_pointer = open('key', 'r')
|
||||
for i in file_pointer:
|
||||
_KEYS.append(i.rstrip())
|
||||
file_pointer.close()
|
||||
|
||||
|
||||
class GMapsStitcher(object):
|
||||
def __init__(self, width, height,
|
||||
latitude, longitude, zoom,
|
||||
maptype, radius_meters=None, num_tiles=4, debug=False):
|
||||
self.helper = MapHelper.MapHelper()
|
||||
self.latitude = latitude
|
||||
self.longitude = longitude
|
||||
self.start_latitude = latitude
|
||||
self.start_longitude = longitude
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.zoom = zoom
|
||||
self.maptype = maptype
|
||||
self.radius_meters = radius_meters
|
||||
self.num_tiles = num_tiles
|
||||
self.display_image = self.helper.new_image(width, height)
|
||||
self.debug = debug
|
||||
|
||||
# Get the big image here
|
||||
self._fetch()
|
||||
self.center_display(latitude, longitude)
|
||||
|
||||
def __str__(self):
|
||||
"""
|
||||
This string returns when used in a print statement
|
||||
Useful for debugging and to print current state
|
||||
|
||||
returns STRING
|
||||
"""
|
||||
string_builder = ""
|
||||
string_builder += ("Center of the displayed map: %4f, %4f\n" %
|
||||
(self.center_x, self.center_y))
|
||||
string_builder += ("Center of the big map: %4fx%4f\n" %
|
||||
(self.start_longitude, self.start_longitude))
|
||||
string_builder += ("Current latitude is: %4f, %4f\n" %
|
||||
(self.longitude, self.latitude))
|
||||
string_builder += ("The top-left of the box: %dx%d\n" %
|
||||
(self.left_x, self.upper_y))
|
||||
string_builder += ("Number of tiles genreated: %dx%d\n" %
|
||||
(self.num_tiles, self.num_tiles))
|
||||
string_builder += "Map Type: %s\n" % (self.maptype)
|
||||
string_builder += "Zoom Level: %s\n" % (self.zoom)
|
||||
string_builder += ("Dimensions of Big Image: %dx%d\n" %
|
||||
(self.big_image.size[0], self.big_image.size[1]))
|
||||
string_builder += ("Dimensions of Displayed Image: %dx%d\n" %
|
||||
(self.width, self.height))
|
||||
string_builder += ("LatLong of Northwest Corner: %4f, %4f\n" %
|
||||
(self.northwest))
|
||||
string_builder += ("LatLong of Southeast Corner: %4f, %4f\n" %
|
||||
(self.southeast))
|
||||
return string_builder
|
||||
|
||||
def _grab_tile(self, longitude, latitude, sleeptime=0):
|
||||
"""
|
||||
This will return the tile at location longitude x latitude.
|
||||
Includes a sleep time to allow for free use if there is no API key
|
||||
|
||||
returns PIL.IMAGE OBJECT
|
||||
"""
|
||||
# Make the url string for polling
|
||||
# GET request header gets appended to the string
|
||||
urlbase = 'https://maps.googleapis.com/maps/api/staticmap?'
|
||||
urlbase += 'center=%.4f,%.4f&zoom=%d&maptype=%s'
|
||||
urlbase += '&size=%dx%d&format=png&key=%s'
|
||||
|
||||
# Fill the formatting
|
||||
specs = (self.helper.fast_round(latitude, _DEGREE_PRECISION),
|
||||
self.helper.fast_round(longitude, _DEGREE_PRECISION),
|
||||
self.zoom, self.maptype, _TILESIZE, _TILESIZE, _KEYS[0])
|
||||
filename = 'Resources/Maps/' + ('%.4f_%.4f_%d_%s_%d_%d_%s' % specs)
|
||||
filename += '.png'
|
||||
|
||||
# Tile Image object
|
||||
tile_object = None
|
||||
|
||||
if os.path.isfile(filename):
|
||||
tile_object = PIL.Image.open(filename)
|
||||
|
||||
# If file on filesystem
|
||||
else:
|
||||
# make the url
|
||||
url = urlbase % specs
|
||||
url = signing.sign_url(url, _KEYS[1])
|
||||
result = urllib2.urlopen(urllib2.Request(url)).read()
|
||||
tile_object = PIL.Image.open(BytesIO(result))
|
||||
if not os.path.exists('Resources/Maps'):
|
||||
os.mkdir('Resources/Maps')
|
||||
tile_object.save(filename)
|
||||
# Added to prevent timeouts on Google Servers
|
||||
time.sleep(sleeptime)
|
||||
|
||||
return tile_object
|
||||
|
||||
def _pixels_to_lon(self, iterator, lon_pixels):
|
||||
"""
|
||||
This converts pixels to degrees to be used in
|
||||
fetching squares and generate correct squares
|
||||
|
||||
returns FLOAT(degrees)
|
||||
"""
|
||||
# Magic Lines, no idea
|
||||
degrees = self.helper.pixels_to_degrees(
|
||||
(iterator - self.num_tiles / 2) * _TILESIZE, self.zoom)
|
||||
return math.degrees((lon_pixels + degrees - _EARTHPIX) / _PIXRAD)
|
||||
|
||||
def _pixels_to_lat(self, iterator, lat_pixels):
|
||||
"""
|
||||
This converts pixels to latitude using meridian projection
|
||||
to get the latitude to generate squares
|
||||
|
||||
returns FLOAT(degrees)
|
||||
"""
|
||||
# Magic Lines
|
||||
return math.degrees(math.pi / 2 - 2 * math.atan(math.exp(((lat_pixels +
|
||||
self.helper.pixels_to_degrees(
|
||||
(iterator - self.num_tiles /
|
||||
2) * _TILESIZE, self.zoom)) -
|
||||
_EARTHPIX) / _PIXRAD)))
|
||||
|
||||
def fetch_tiles(self):
|
||||
"""
|
||||
Function that handles fetching of files from init'd variables
|
||||
|
||||
returns PIL.IMAGE OBJECT, (WEST, NORTH), (EAST, SOUTH)
|
||||
|
||||
North/East/South/West are in FLOAT(degrees)
|
||||
"""
|
||||
# cap floats to precision amount
|
||||
self.latitude = self.helper.fast_round(self.latitude,
|
||||
_DEGREE_PRECISION)
|
||||
self.longitude = self.helper.fast_round(self.longitude,
|
||||
_DEGREE_PRECISION)
|
||||
|
||||
# number of tiles required to go from center
|
||||
# latitude to desired radius in meters
|
||||
if self.radius_meters is not None:
|
||||
self.num_tiles = (int(
|
||||
round(2 * self.helper.pixels_to_meters(
|
||||
self.latitude, self.zoom) /
|
||||
(_TILESIZE / 2. / self.radius_meters))))
|
||||
|
||||
lon_pixels = _EARTHPIX + self.longitude * math.radians(_PIXRAD)
|
||||
|
||||
sin_lat = math.sin(math.radians(self.latitude))
|
||||
lat_pixels = _EARTHPIX - _PIXRAD * math.log((1+sin_lat)/(1-sin_lat))/2
|
||||
self.big_size = self.num_tiles * _TILESIZE
|
||||
big_image = self.helper.new_image(self.big_size, self.big_size)
|
||||
|
||||
for j in range(self.num_tiles):
|
||||
lon = self._pixels_to_lon(j, lon_pixels)
|
||||
for k in range(self.num_tiles):
|
||||
lat = self._pixels_to_lat(k, lat_pixels)
|
||||
tile = self._grab_tile(lon, lat)
|
||||
big_image.paste(tile, (j * _TILESIZE, k * _TILESIZE))
|
||||
|
||||
west = self._pixels_to_lon(0, lon_pixels)
|
||||
east = self._pixels_to_lon(self.num_tiles - 1, lon_pixels)
|
||||
|
||||
north = self._pixels_to_lat(0, lat_pixels)
|
||||
south = self._pixels_to_lat(self.num_tiles - 1, lat_pixels)
|
||||
return big_image, (north, west), (south, east)
|
||||
|
||||
def move_pix(self, dx, dy):
|
||||
"""
|
||||
Function gets change in x and y (dx, dy)
|
||||
then displaces the displayed map that amount
|
||||
|
||||
NO RETURN
|
||||
"""
|
||||
self._constrain_x(dx)
|
||||
self._constrain_y(dy)
|
||||
self.update()
|
||||
|
||||
def _constrain_x(self, diff):
|
||||
"""
|
||||
Helper for move_pix
|
||||
"""
|
||||
new_value = self.left_x - diff
|
||||
|
||||
if ((not new_value > 0) and
|
||||
(new_value < self.big_image.size[0] - self.width)):
|
||||
return self.left_x
|
||||
else:
|
||||
return new_value
|
||||
|
||||
def _constrain_y(self, diff):
|
||||
"""
|
||||
Helper for move_pix
|
||||
"""
|
||||
new_value = self.upper_y - diff
|
||||
|
||||
if ((not new_value > 0) and
|
||||
(new_value < self.big_image.size[1] - self.height)):
|
||||
return self.upper_y
|
||||
else:
|
||||
return new_value
|
||||
|
||||
def update(self):
|
||||
"""
|
||||
Function remakes display image using top left corners
|
||||
"""
|
||||
self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
|
||||
# self.display_image.resize((self.image_zoom, self.image_zoom))
|
||||
|
||||
def _fetch(self):
|
||||
"""
|
||||
Function generates big image
|
||||
"""
|
||||
self.big_image, self.northwest, self.southeast = self.fetch_tiles()
|
||||
|
||||
def move_latlon(self, lat, lon):
|
||||
"""
|
||||
Function to move the object/rover
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
self._constrain_x(self.center_x-x)
|
||||
self._constrain_y(self.center_y-y)
|
||||
self.update()
|
||||
|
||||
def _get_cartesian(self, lat, lon):
|
||||
"""
|
||||
Helper for getting the x, y given lat and lon
|
||||
|
||||
returns INT, INT (x, y)
|
||||
"""
|
||||
viewport_lat_nw, viewport_lon_nw = self.northwest
|
||||
viewport_lat_se, viewport_lon_se = self.southeast
|
||||
# print "Lat:", viewport_lat_nw, viewport_lat_se
|
||||
# print "Lon:", viewport_lon_nw, viewport_lon_se
|
||||
|
||||
viewport_lat_diff = viewport_lat_nw - viewport_lat_se
|
||||
viewport_lon_diff = viewport_lon_se - viewport_lon_nw
|
||||
|
||||
# print viewport_lon_diff, viewport_lat_diff
|
||||
|
||||
bigimage_width = self.big_image.size[0]
|
||||
bigimage_height = self.big_image.size[1]
|
||||
|
||||
pixel_per_lat = bigimage_height / viewport_lat_diff
|
||||
pixel_per_lon = bigimage_width / viewport_lon_diff
|
||||
# print "Pixel per:", pixel_per_lat, pixel_per_lon
|
||||
|
||||
new_lat_gps_range_percentage = (viewport_lat_nw - lat)
|
||||
new_lon_gps_range_percentage = (lon - viewport_lon_nw)
|
||||
# print lon, viewport_lon_se
|
||||
|
||||
x = new_lon_gps_range_percentage * pixel_per_lon
|
||||
y = new_lat_gps_range_percentage * pixel_per_lat
|
||||
|
||||
return int(x), int(y)
|
||||
|
||||
def add_gps_location(self, lat, lon, shape, size, fill):
|
||||
"""
|
||||
Function adds a shape at lat x lon
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
draw = PIL.ImageDraw.Draw(self.big_image)
|
||||
if shape is "ellipsis":
|
||||
draw.ellipsis((x-size, y-size, x+size, y+size), fill)
|
||||
else:
|
||||
draw.rectangle([x-size, y-size, x+size, y+size], fill)
|
||||
self.update()
|
||||
|
||||
def center_display(self, lat, lon):
|
||||
"""
|
||||
Function centers the display image
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
self.center_x = x
|
||||
self.center_y = y
|
||||
|
||||
self.left_x = (self.center_x - (self.width/2))
|
||||
self.upper_y = (self.center_y - (self.height/2))
|
||||
self.update()
|
||||
|
||||
# def update_rover_map_location(self, lat, lon):
|
||||
# print "I did nothing"
|
||||
|
||||
# def draw_circle(self, lat, lon, radius, fill):
|
||||
# print "I did nothing"
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
|
||||
class OverlayImage(object):
|
||||
def __init__(self, latitude, longitude, northwest, southeast,
|
||||
big_width, big_height, width, height):
|
||||
self.northwest = northwest
|
||||
self.southeast = southeast
|
||||
self.latitude = latitude
|
||||
self.longitude = longitude
|
||||
self.big_width = big_width
|
||||
self.big_height = big_height
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.big_image = None
|
||||
self.display_image = None
|
||||
self.helper = MapHelper.MapHelper()
|
||||
|
||||
x, y = self._get_cartesian(latitude, longitude)
|
||||
self.center_x = x
|
||||
self.center_y = y
|
||||
|
||||
self.left_x = (self.center_x - (self.width/2))
|
||||
self.upper_y = (self.center_y - (self.height/2))
|
||||
|
||||
def generate_image_files(self):
|
||||
"""
|
||||
Creates big_image and display image sizes
|
||||
|
||||
Returns NONE
|
||||
"""
|
||||
self.big_image = self.helper.new_image(self.big_width, self.big_height,
|
||||
True)
|
||||
self.display_image = self.helper.new_image(self.width, self.height,
|
||||
True)
|
||||
|
||||
def _get_cartesian(self, lat, lon):
|
||||
"""
|
||||
Helper for getting the x, y given lat and lon
|
||||
|
||||
returns INT, INT (x, y)
|
||||
"""
|
||||
viewport_lat_nw, viewport_lon_nw = self.northwest
|
||||
viewport_lat_se, viewport_lon_se = self.southeast
|
||||
# print "Lat:", viewport_lat_nw, viewport_lat_se
|
||||
# print "Lon:", viewport_lon_nw, viewport_lon_se
|
||||
|
||||
viewport_lat_diff = viewport_lat_nw - viewport_lat_se
|
||||
viewport_lon_diff = viewport_lon_se - viewport_lon_nw
|
||||
|
||||
# print viewport_lon_diff, viewport_lat_diff
|
||||
|
||||
bigimage_width = self.width
|
||||
bigimage_height = self.height
|
||||
|
||||
pixel_per_lat = bigimage_height / viewport_lat_diff
|
||||
pixel_per_lon = bigimage_width / viewport_lon_diff
|
||||
# print "Pixel per:", pixel_per_lat, pixel_per_lon
|
||||
|
||||
new_lat_gps_range_percentage = (viewport_lat_nw - lat)
|
||||
new_lon_gps_range_percentage = (lon - viewport_lon_nw)
|
||||
# print lon, viewport_lon_se
|
||||
|
||||
x = new_lon_gps_range_percentage * pixel_per_lon
|
||||
y = new_lat_gps_range_percentage * pixel_per_lat
|
||||
|
||||
return int(x), int(y)
|
||||
|
||||
def update_new_location(self, latitude, longitude, compass):
|
||||
self._draw_rover(latitude, longitude, 10, compass)
|
||||
self.update()
|
||||
|
||||
return self.display_image
|
||||
|
||||
def _draw_rover(self, lat, lon, size, scaler):
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
draw = PIL.ImageDraw.Draw(self.big_image)
|
||||
draw.ellipsis((x-size, y-size, x+size, y+size), (255, 255, 255, 0))
|
||||
draw.line(
|
||||
math.cos(x-2*size * scaler),
|
||||
math.sin(y * scaler),
|
||||
math.cos(x * scaler),
|
||||
math.sin(y+2*size * scaler), (255, 255, 255, 0), 25)
|
||||
draw.line(
|
||||
math.cos(x+2*size * scaler),
|
||||
math.sin(y * scaler),
|
||||
math.cos(x * scaler),
|
||||
math.sin(y+2*size * scaler), (255, 255, 255, 0), 25)
|
||||
|
||||
def update(self):
|
||||
self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
|
||||
@@ -0,0 +1,104 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtWidgets, QtGui
|
||||
from PIL.ImageQt import ImageQt
|
||||
|
||||
import logging
|
||||
|
||||
import rospy
|
||||
|
||||
# Custom Imports
|
||||
import RoverMap
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
# put some stuff here later so you can remember
|
||||
|
||||
|
||||
class RoverMapCoordinator(QtCore.QThread):
|
||||
pixmap_ready_signal = QtCore.pyqtSignal()
|
||||
|
||||
def __init__(self, shared_objects):
|
||||
super(RoverMapCoordinator, self).__init__()
|
||||
|
||||
self.shared_objects = shared_objects
|
||||
self.left_screen = self.shared_objects["screens"]["left_screen"]
|
||||
self.mapping_label = self.left_screen.mapping_label
|
||||
|
||||
self.setings = QtCore.QSettings()
|
||||
|
||||
self.logger = logging.getLogger("groundstation")
|
||||
|
||||
self.run_thread_flag = True
|
||||
self.setup_map_flag = True
|
||||
|
||||
self.google_maps_object = None
|
||||
self.map_image = None
|
||||
self.overlay_image = None
|
||||
self.overlay_image_object = None
|
||||
|
||||
self.map_pixmap = None
|
||||
|
||||
def run(self):
|
||||
self.logger.debug("Starting Map Coordinator Thread")
|
||||
|
||||
while self.run_thread_flag:
|
||||
if self.setup_map_flag:
|
||||
self._map_setup()
|
||||
self.setup_map_flag = False
|
||||
else:
|
||||
self._get_map_image()
|
||||
self.msleep(3)
|
||||
|
||||
self.logger.debug("Stopping Map Coordinator Thread")
|
||||
|
||||
def _setup_map_threads(self):
|
||||
self.google_maps_object = RoverMap.GMapsStitcher(1280,
|
||||
720,
|
||||
44.567161,
|
||||
-123.278432,
|
||||
18,
|
||||
'terrain',
|
||||
None, 20)
|
||||
|
||||
def _map_setup(self):
|
||||
self.google_maps_object = RoverMap.GMapsStitcher(1280,
|
||||
720,
|
||||
44.567161,
|
||||
-123.278432,
|
||||
18,
|
||||
'terrain',
|
||||
None, 20)
|
||||
self.overlay_image_object = (
|
||||
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],
|
||||
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)
|
||||
# get overlay here
|
||||
qim = ImageQt(self.map_image)
|
||||
self.map_pixmap = QtGui.QPixmap.fromImage(qim)
|
||||
self.pixmap_ready_signal.emit()
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
self.pixmap_ready_signal.connect(self.pixmap_ready__slot)
|
||||
|
||||
def on_kill_threads_requested_slot(self):
|
||||
self.run_thread_flag = False
|
||||
|
||||
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 pixmap_ready__slot(self):
|
||||
self.mapping_label.setPixmap(self.map_pixmap)
|
||||
@@ -0,0 +1,46 @@
|
||||
import PIL.Image
|
||||
import math
|
||||
|
||||
|
||||
class MapHelper(object):
|
||||
|
||||
@staticmethod
|
||||
def new_image(width, height, alpha=False):
|
||||
"""
|
||||
Generates a new image using PIL.Image module
|
||||
|
||||
returns PIL.IMAGE OBJECT
|
||||
"""
|
||||
if alpha is True:
|
||||
return PIL.Image.new('RGBA', (width, height), (0, 0, 0, 255))
|
||||
else:
|
||||
return PIL.Image.new('RGBA', (width, height))
|
||||
|
||||
@staticmethod
|
||||
def fast_round(value, precision):
|
||||
"""
|
||||
Function to round values instead of using python's
|
||||
|
||||
return INT
|
||||
"""
|
||||
return int(value * 10 ** precision) / 10. ** precision
|
||||
|
||||
@staticmethod
|
||||
def pixels_to_degrees(pixels, zoom):
|
||||
"""
|
||||
Generates pixels to be expected at zoom levels
|
||||
|
||||
returns INT
|
||||
"""
|
||||
return pixels * 2 ** (21-zoom)
|
||||
|
||||
@staticmethod
|
||||
def pixels_to_meters(latitude, zoom):
|
||||
"""
|
||||
Function generates how many pixels per meter it
|
||||
should be from the projecction
|
||||
|
||||
returns FLOAT
|
||||
"""
|
||||
# https://groups.google.com/forum/#!topic/google-maps-js-api-v3/hDRO4oHVSeM
|
||||
return 2 ** zoom / (156543.03392 * math.cos(math.radians(latitude)))
|
||||
@@ -0,0 +1,53 @@
|
||||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
""" Signs a URL using a URL signing secret """
|
||||
|
||||
import hashlib
|
||||
import hmac
|
||||
import base64
|
||||
import urlparse
|
||||
|
||||
def sign_url(input_url=None, secret=None):
|
||||
""" Sign a request URL with a URL signing secret.
|
||||
|
||||
Usage:
|
||||
from urlsigner import sign_url
|
||||
|
||||
signed_url = sign_url(input_url=my_url, secret=SECRET)
|
||||
|
||||
Args:
|
||||
input_url - The URL to sign
|
||||
secret - Your URL signing secret
|
||||
|
||||
Returns:
|
||||
The signed request URL
|
||||
"""
|
||||
|
||||
if not input_url or not secret:
|
||||
raise Exception("Both input_url and secret are required")
|
||||
|
||||
url = urlparse.urlparse(input_url)
|
||||
|
||||
# We only need to sign the path+query part of the string
|
||||
url_to_sign = url.path + "?" + url.query
|
||||
|
||||
# Decode the private key into its binary format
|
||||
# We need to decode the URL-encoded private key
|
||||
decoded_key = base64.urlsafe_b64decode(secret)
|
||||
|
||||
# Create a signature using the private key and the URL-encoded
|
||||
# string using HMAC SHA1. This signature will be binary.
|
||||
signature = hmac.new(decoded_key, url_to_sign, hashlib.sha1)
|
||||
|
||||
# Encode the binary signature into base64 for use within a URL
|
||||
encoded_signature = base64.urlsafe_b64encode(signature.digest())
|
||||
|
||||
original_url = url.scheme + "://" + url.netloc + url.path + "?" + url.query
|
||||
|
||||
# Return signed URL
|
||||
return original_url + "&signature=" + encoded_signature
|
||||
|
||||
if __name__ == "__main__":
|
||||
input_url = raw_input("URL to Sign: ")
|
||||
secret = raw_input("URL signing secret: ")
|
||||
print "Signed URL: " + sign_url(input_url, secret)
|
||||
@@ -0,0 +1,45 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtGui, QtWidgets
|
||||
import time
|
||||
import logging
|
||||
import socket
|
||||
|
||||
import rospy
|
||||
|
||||
# Custom Imports
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
|
||||
|
||||
#####################################
|
||||
# RoverVideoReceiver Class Definition
|
||||
#####################################
|
||||
class ROSMasterChecker(QtCore.QThread):
|
||||
def __init__(self):
|
||||
super(ROSMasterChecker, self).__init__()
|
||||
|
||||
# ########## Class Variables ##########
|
||||
self.ros_master_present = False
|
||||
|
||||
self.start_time = time.time()
|
||||
self.start()
|
||||
|
||||
def run(self):
|
||||
try:
|
||||
master = rospy.get_master()
|
||||
master.getPid()
|
||||
self.ros_master_present = True
|
||||
except socket.error:
|
||||
return
|
||||
|
||||
def master_present(self, timeout):
|
||||
while self.isRunning() and (time.time() - self.start_time) < timeout:
|
||||
self.msleep(100)
|
||||
|
||||
return self.ros_master_present
|
||||
|
||||
@@ -0,0 +1,199 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtWidgets
|
||||
import logging
|
||||
|
||||
import rospy
|
||||
|
||||
# Custom Imports
|
||||
import RoverVideoReceiver
|
||||
|
||||
#####################################
|
||||
# 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)
|
||||
|
||||
|
||||
#####################################
|
||||
# RoverVideoCoordinator Class Definition
|
||||
#####################################
|
||||
class RoverVideoCoordinator(QtCore.QThread):
|
||||
pixmap_ready_signal = QtCore.pyqtSignal(str)
|
||||
|
||||
def __init__(self, shared_objects):
|
||||
super(RoverVideoCoordinator, 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
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
|
||||
# ########## Get the Pick And Plate instance of the logger ##########
|
||||
self.logger = logging.getLogger("groundstation")
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
self.setup_cameras_flag = True
|
||||
|
||||
# ########## Class Variables ##########
|
||||
# Camera variables
|
||||
self.camera_threads = {}
|
||||
self.valid_cameras = []
|
||||
self.disabled_cameras = []
|
||||
|
||||
# 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.primary_label_max_resolution = -1
|
||||
self.secondary_label_max_resolution = -1
|
||||
self.tertiary_label_max_resolution = -1
|
||||
|
||||
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()
|
||||
self.msleep(10)
|
||||
|
||||
self.__wait_for_camera_threads()
|
||||
|
||||
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.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_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_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)
|
||||
|
||||
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})
|
||||
|
||||
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)
|
||||
|
||||
def __get_cameras(self):
|
||||
topics = rospy.get_published_topics(CAMERA_TOPIC_PATH)
|
||||
|
||||
names = []
|
||||
|
||||
for topics_group in topics:
|
||||
main_topic = topics_group[0]
|
||||
if "heartbeat" in main_topic:
|
||||
continue
|
||||
camera_name = main_topic.split("/")[2]
|
||||
names.append(camera_name)
|
||||
|
||||
names = set(names)
|
||||
|
||||
for camera in EXCLUDED_CAMERAS:
|
||||
if camera in names:
|
||||
names.remove(camera)
|
||||
|
||||
self.valid_cameras = list(names)
|
||||
|
||||
def __setup_video_threads(self):
|
||||
for camera in self.valid_cameras:
|
||||
self.camera_threads[camera] = RoverVideoReceiver.RoverVideoReceiver(camera)
|
||||
|
||||
def __wait_for_camera_threads(self):
|
||||
for camera in self.camera_threads:
|
||||
self.camera_threads[camera].wait()
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
for thread in self.camera_threads:
|
||||
self.camera_threads[thread].image_ready_signal.connect(self.pixmap_ready__slot)
|
||||
|
||||
self.primary_video_display_label.mousePressEvent = self.__change_display_source_primary_mouse_press_event
|
||||
self.secondary_video_display_label.mousePressEvent = self.__change_display_source_secondary_mouse_press_event
|
||||
self.tertiary_video_display_label.mousePressEvent = self.__change_display_source_tertiary_mouse_press_event
|
||||
|
||||
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)
|
||||
|
||||
for camera in self.camera_threads:
|
||||
self.camera_threads[camera].setup_signals(start_signal, signals_and_slots_signal, kill_signal)
|
||||
|
||||
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)
|
||||
elif event.button() == QtCore.Qt.RightButton:
|
||||
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)
|
||||
elif event.button() == QtCore.Qt.RightButton:
|
||||
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)
|
||||
elif event.button() == QtCore.Qt.RightButton:
|
||||
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)
|
||||
|
||||
if self.valid_cameras[self.secondary_label_current_setting] == camera:
|
||||
self.secondary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
|
||||
|
||||
if self.valid_cameras[self.tertiary_label_current_setting] == camera:
|
||||
self.tertiary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
|
||||
|
||||
def on_kill_threads_requested__slot(self):
|
||||
self.run_thread_flag = False
|
||||
@@ -0,0 +1,256 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtGui, QtWidgets
|
||||
import logging
|
||||
import cv2
|
||||
import numpy as np
|
||||
import qimage2ndarray
|
||||
|
||||
import rospy
|
||||
import dynamic_reconfigure.client
|
||||
from cv_bridge import CvBridge
|
||||
from sensor_msgs.msg import CompressedImage
|
||||
|
||||
# Custom Imports
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
CAMERA_TOPIC_PATH = "/cameras/"
|
||||
|
||||
QUALITY_MAX = 80
|
||||
QUALITY_MIN = 15
|
||||
|
||||
|
||||
#####################################
|
||||
# RoverVideoReceiver Class Definition
|
||||
#####################################
|
||||
class RoverVideoReceiver(QtCore.QThread):
|
||||
image_ready_signal = QtCore.pyqtSignal(str)
|
||||
|
||||
def __init__(self, camera_name):
|
||||
super(RoverVideoReceiver, self).__init__()
|
||||
|
||||
# ########## Reference to class init variables ##########
|
||||
self.camera_name = camera_name
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
|
||||
# ########## Get the Pick And Plate instance of the logger ##########
|
||||
self.logger = logging.getLogger("groundstation")
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
# ########## Class Variables ##########
|
||||
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"
|
||||
|
||||
# Subscription variables
|
||||
self.video_subscriber = None # type: rospy.Subscriber
|
||||
|
||||
# Image variables
|
||||
self.raw_image = None
|
||||
self.opencv_1280x720_image = None
|
||||
self.opencv_640x360_image = None
|
||||
|
||||
self.pixmap_1280x720_image = None
|
||||
self.pixmap_640x360_image = None
|
||||
|
||||
# Processing variables
|
||||
self.bridge = CvBridge() # OpenCV ROS Video Data Processor
|
||||
self.video_enabled = True
|
||||
self.new_frame = False
|
||||
|
||||
# Text Drawing Variables
|
||||
self.font = cv2.FONT_HERSHEY_TRIPLEX
|
||||
self.font_thickness = 1
|
||||
self.font_baseline = 0
|
||||
|
||||
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)
|
||||
|
||||
while self.run_thread_flag:
|
||||
if self.video_enabled:
|
||||
self.__show_video_enabled()
|
||||
else:
|
||||
self.__show_video_disabled()
|
||||
|
||||
self.msleep(10)
|
||||
|
||||
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 __set_jpeg_quality(self, quality_setting):
|
||||
self.reconfigure_clients[self.current_camera_settings["resolution"]].update_configuration({"jpeg_quality": quality_setting})
|
||||
|
||||
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)
|
||||
|
||||
def __get_camera_available_resolutions(self):
|
||||
topics = rospy.get_published_topics(self.topic_base_path)
|
||||
|
||||
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"]
|
||||
|
||||
def __show_video_enabled(self):
|
||||
self.__update_camera_subscription_and_settings()
|
||||
|
||||
if self.new_frame and self.current_camera_settings["resolution"]:
|
||||
# self.__perform_quality_check_and_adjust()
|
||||
|
||||
opencv_image = self.bridge.compressed_imgmsg_to_cv2(self.raw_image, "rgb8")
|
||||
|
||||
self.__create_final_pixmaps(opencv_image)
|
||||
|
||||
self.image_ready_signal.emit(self.camera_name)
|
||||
self.new_frame = False
|
||||
|
||||
def __show_video_disabled(self):
|
||||
blank_frame = np.zeros((720, 1280, 3), np.uint8)
|
||||
|
||||
self.__create_final_pixmaps(blank_frame)
|
||||
|
||||
self.image_ready_signal.emit(self.camera_name)
|
||||
|
||||
def __create_final_pixmaps(self, opencv_image):
|
||||
height, width, _ = opencv_image.shape
|
||||
|
||||
if width != 1280 and height != 720:
|
||||
self.opencv_1280x720_image = cv2.resize(opencv_image, (1280, 720))
|
||||
else:
|
||||
self.opencv_1280x720_image = opencv_image
|
||||
|
||||
if width != 640 and height != 360:
|
||||
self.opencv_640x360_image = cv2.resize(opencv_image, (640, 360))
|
||||
else:
|
||||
self.opencv_640x360_image = opencv_image
|
||||
|
||||
self.__apply_camera_name(self.opencv_1280x720_image, self.camera_name_opencv_1280x720_image)
|
||||
self.__apply_camera_name(self.opencv_640x360_image, self.camera_name_opencv_640x360_image)
|
||||
|
||||
self.pixmap_1280x720_image = QtGui.QPixmap.fromImage(qimage2ndarray.array2qimage(
|
||||
self.opencv_1280x720_image))
|
||||
self.pixmap_640x360_image = QtGui.QPixmap.fromImage(qimage2ndarray.array2qimage(
|
||||
self.opencv_640x360_image))
|
||||
|
||||
def __image_data_received_callback(self, raw_image):
|
||||
self.raw_image = raw_image
|
||||
self.new_frame = True
|
||||
|
||||
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]
|
||||
|
||||
camera_name_width_buffered = camera_name_text_width + 10
|
||||
camera_name_height_buffered = camera_name_text_height + 20
|
||||
|
||||
camera_name_opencv_image = np.zeros(
|
||||
(camera_name_height_buffered, camera_name_width_buffered, 3), np.uint8)
|
||||
|
||||
cv2.putText(
|
||||
camera_name_opencv_image,
|
||||
self.camera_title_name,
|
||||
((camera_name_width_buffered - camera_name_text_width) / 2, int((camera_name_height_buffered * 2) / 3)),
|
||||
self.font,
|
||||
1,
|
||||
(255, 255, 255),
|
||||
1,
|
||||
cv2.LINE_AA)
|
||||
|
||||
self.camera_name_opencv_1280x720_image = \
|
||||
cv2.resize(camera_name_opencv_image, (camera_name_width_buffered, camera_name_height_buffered))
|
||||
|
||||
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 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)
|
||||
self.video_enabled = True
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
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 on_kill_threads_requested__slot(self):
|
||||
self.run_thread_flag = False
|
||||
|
||||
@staticmethod
|
||||
def __apply_camera_name(opencv_image, font_opencv_image):
|
||||
opencv_image[0:0 + font_opencv_image.shape[0], 0:0 + font_opencv_image.shape[1]] = \
|
||||
font_opencv_image
|
||||
@@ -0,0 +1,134 @@
|
||||
import sys
|
||||
from PyQt5 import QtWidgets, QtCore, QtGui, uic
|
||||
import signal
|
||||
import rospy
|
||||
import time
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
import cv2
|
||||
import qimage2ndarray
|
||||
import numpy as np
|
||||
|
||||
from geometry_msgs.msg import Twist
|
||||
from sensor_msgs.msg import CompressedImage
|
||||
#from sensor_msgs.msg import Image, CompressedImage
|
||||
|
||||
|
||||
class VideoTest(QtCore.QThread):
|
||||
publish_message_signal = QtCore.pyqtSignal()
|
||||
image_ready_signal = QtCore.pyqtSignal()
|
||||
|
||||
def __init__(self, shared_objects, screen_label, video_size=None, sub_path=None):
|
||||
super(VideoTest, self).__init__()
|
||||
|
||||
self.not_abort = True
|
||||
|
||||
self.shared_objects = shared_objects
|
||||
|
||||
self.right_screen_label = screen_label # type: QtGui.QPixmap
|
||||
self.video_size = video_size
|
||||
|
||||
self.message = None
|
||||
|
||||
self.publisher = rospy.Subscriber(sub_path, CompressedImage, self.__receive_message)
|
||||
|
||||
self.raw_image = None
|
||||
self.cv_image = None
|
||||
self.pixmap = None
|
||||
self.bridge = CvBridge()
|
||||
# self.bridge.com
|
||||
|
||||
self.new_frame = False
|
||||
self.frame_count = 0
|
||||
self.last_frame_time = time.time()
|
||||
self.fps = 0
|
||||
|
||||
self.name = sub_path.split("/")[2].replace("_", " ").title()
|
||||
|
||||
self.font = cv2.FONT_HERSHEY_TRIPLEX
|
||||
|
||||
thickness = 1
|
||||
baseline = 0
|
||||
|
||||
text_size = cv2.getTextSize(self.name, self.font, thickness, baseline)
|
||||
print text_size
|
||||
|
||||
text_width, text_height = text_size[0]
|
||||
|
||||
width = text_width + 10
|
||||
height = text_height + 20
|
||||
|
||||
self.blank_image = np.zeros((height, width, 3), np.uint8)
|
||||
cv2.putText(self.blank_image, self.name, ((width - text_width) / 2, int((height * 2) / 3)), self.font, 1, (255, 255, 255), 1, cv2.LINE_AA)
|
||||
self.blank_image = cv2.resize(self.blank_image, (width / 2, height / 2))
|
||||
|
||||
def run(self):
|
||||
# TODO: Thread starting message here
|
||||
y_offset = 0
|
||||
x_offset = 0
|
||||
|
||||
while self.not_abort:
|
||||
if self.raw_image and self.new_frame:
|
||||
self.cv_image = self.bridge.compressed_imgmsg_to_cv2(self.raw_image, "rgb8")
|
||||
|
||||
self.cv_image = self.__show_fps(self.cv_image)
|
||||
|
||||
self.cv_image[y_offset:y_offset + self.blank_image.shape[0], x_offset:x_offset + self.blank_image.shape[1]] = self.blank_image
|
||||
|
||||
if self.video_size:
|
||||
self.cv_image = cv2.resize(self.cv_image, self.video_size)
|
||||
self.pixmap = QtGui.QPixmap.fromImage(qimage2ndarray.array2qimage(self.cv_image))
|
||||
self.image_ready_signal.emit()
|
||||
self.new_frame = False
|
||||
|
||||
if (time.time() - self.last_frame_time) >= 0.5:
|
||||
self.fps = int(self.frame_count / (time.time() - self. last_frame_time))
|
||||
|
||||
self.last_frame_time = time.time()
|
||||
self.frame_count = 0
|
||||
|
||||
self.msleep(18)
|
||||
# TODO: Thread ending message here
|
||||
|
||||
def __show_fps(self, image):
|
||||
thickness = 1
|
||||
baseline = 0
|
||||
|
||||
fps_string = str(self.fps)
|
||||
|
||||
text_size = cv2.getTextSize(fps_string, self.font, thickness, baseline)
|
||||
|
||||
text_width, text_height = text_size[0]
|
||||
|
||||
width = text_width + 10
|
||||
height = text_height + 20
|
||||
|
||||
fps_image = np.zeros((height, width, 3), np.uint8)
|
||||
|
||||
cv2.putText(fps_image, fps_string, ((width - text_width) / 2, int((height * 2) / 3)), self.font, 1, (255, 255, 255), 1, cv2.LINE_AA)
|
||||
fps_image = cv2.resize(fps_image, (width / 2, height / 2))
|
||||
|
||||
y_offset = 0
|
||||
x_offset = (image.shape[1] - fps_image.shape[1]) / 2
|
||||
|
||||
image[y_offset:y_offset + fps_image.shape[0], x_offset:x_offset + fps_image.shape[1]] = fps_image
|
||||
|
||||
return image
|
||||
|
||||
def __on_image_update_ready(self):
|
||||
self.right_screen_label.setPixmap(self.pixmap)
|
||||
|
||||
def __receive_message(self, message):
|
||||
self.raw_image = message
|
||||
self.new_frame = True
|
||||
self.frame_count += 1
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
self.image_ready_signal.connect(self.__on_image_update_ready)
|
||||
|
||||
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 on_kill_threads_requested__slot(self):
|
||||
self.not_abort = False
|
||||
14
software/ros_packages/ground_station/src/Readme.md
Normal file
14
software/ros_packages/ground_station/src/Readme.md
Normal file
@@ -0,0 +1,14 @@
|
||||
# Ground Station Software
|
||||
This code is for the ground station side of the Mars Rover project.
|
||||
|
||||
## Requirements
|
||||
* Python 2.X
|
||||
* PyQt5
|
||||
* ROS Kinetic
|
||||
|
||||
Python Libraries
|
||||
* inputs
|
||||
|
||||
## How to Launch
|
||||
Navigate to this directory on a linux system with the requirements listed above installed.
|
||||
Run "python RoverGroundStation.py" or ./RoverGroundStation.py
|
||||
1118
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
Normal file
1118
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
Normal file
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,345 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>MainWindow</class>
|
||||
<widget class="QMainWindow" name="MainWindow">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>MainWindow</string>
|
||||
</property>
|
||||
<property name="windowOpacity">
|
||||
<double>1.000000000000000</double>
|
||||
</property>
|
||||
<widget class="QWidget" name="centralwidget">
|
||||
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="sizeConstraint">
|
||||
<enum>QLayout::SetDefaultConstraint</enum>
|
||||
</property>
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_2">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_4">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QWidget" name="arm_visualization_widget" native="true">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>640</width>
|
||||
<height>720</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>640</width>
|
||||
<height>720</height>
|
||||
</size>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_4">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="sizeConstraint">
|
||||
<enum>QLayout::SetNoConstraint</enum>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QWidget" name="heading_widget" native="true">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>320</width>
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>320</width>
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color:black;;</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout">
|
||||
<property name="margin">
|
||||
<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>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QWidget" name="speed_limit_widget" native="true">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>320</width>
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>320</width>
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout">
|
||||
<item>
|
||||
<widget class="QLabel" name="label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Current Speed</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>2.4 m/s</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_3">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Speed Limit</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QProgressBar" name="progressBar">
|
||||
<property name="value">
|
||||
<number>50</number>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="invertedAppearance">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="textDirection">
|
||||
<enum>QProgressBar::TopToBottom</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_4">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Tank Drive Output</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QProgressBar" name="progressBar_3">
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QProgressBar" name="progressBar_2">
|
||||
<property name="value">
|
||||
<number>0</number>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_3">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QLabel" name="primary_video_label">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>1280</width>
|
||||
<height>720</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>1280</width>
|
||||
<height>720</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: darkblue;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_3">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QLabel" name="secondary_video_label">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>640</width>
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>640</width>
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color:darkgreen;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="tertiary_video_label">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>640</width>
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>640</width>
|
||||
<height>360</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color:maroon;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</widget>
|
||||
<resources/>
|
||||
<connections/>
|
||||
</ui>
|
||||
185
software/ros_packages/ground_station/src/ground_station.py
Normal file
185
software/ros_packages/ground_station/src/ground_station.py
Normal file
@@ -0,0 +1,185 @@
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
import sys
|
||||
from PyQt5 import QtWidgets, QtCore, QtGui, uic
|
||||
import signal
|
||||
import rospy
|
||||
import logging
|
||||
import qdarkstyle
|
||||
import PIL.Image
|
||||
from PIL.ImageQt import ImageQt
|
||||
|
||||
# Custom Imports
|
||||
import Framework.StartupSystems.ROSMasterChecker as ROSMasterChecker
|
||||
import Framework.LoggingSystems.Logger as Logger
|
||||
import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator
|
||||
import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator
|
||||
import Framework.DriveSystems.RoverDriveSender as RoverDriveSender
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
UI_FILE_LEFT = "Resources/Ui/left_screen.ui"
|
||||
UI_FILE_RIGHT = "Resources/Ui/right_screen.ui"
|
||||
|
||||
#####################################
|
||||
# Class Organization
|
||||
#####################################
|
||||
# Class Name:
|
||||
# "init"
|
||||
# "run (if there)" - personal pref
|
||||
# "private methods"
|
||||
# "public methods, minus slots"
|
||||
# "slot methods"
|
||||
# "static methods"
|
||||
# "run (if there)" - personal pref
|
||||
|
||||
|
||||
#####################################
|
||||
# ApplicationWindow Class Definition
|
||||
#####################################
|
||||
class ApplicationWindow(QtWidgets.QMainWindow):
|
||||
exit_requested_signal = QtCore.pyqtSignal()
|
||||
|
||||
kill_threads_signal = QtCore.pyqtSignal()
|
||||
|
||||
def __init__(self, parent=None, ui_file_path=None):
|
||||
super(ApplicationWindow, self).__init__(parent)
|
||||
|
||||
uic.loadUi(ui_file_path, self)
|
||||
|
||||
QtWidgets.QShortcut(QtGui.QKeySequence("Ctrl+Q"), self, self.exit_requested_signal.emit)
|
||||
|
||||
|
||||
#####################################
|
||||
# GroundStation Class Definition
|
||||
#####################################
|
||||
class GroundStation(QtCore.QObject):
|
||||
LEFT_SCREEN_ID = 1
|
||||
RIGHT_SCREEN_ID = 0
|
||||
|
||||
exit_requested_signal = QtCore.pyqtSignal()
|
||||
|
||||
start_threads_signal = QtCore.pyqtSignal()
|
||||
connect_signals_and_slots_signal = QtCore.pyqtSignal()
|
||||
kill_threads_signal = QtCore.pyqtSignal()
|
||||
|
||||
def __init__(self, parent=None,):
|
||||
# noinspection PyArgumentList
|
||||
super(GroundStation, self).__init__(parent)
|
||||
|
||||
# ##### Setup the Logger Immediately #####
|
||||
self.logger_setup_class = Logger.Logger(console_output=True) # Doesn't need to be shared
|
||||
|
||||
# ########## Get the Pick And Plate instance of the logger ##########
|
||||
self.logger = logging.getLogger("groundstation")
|
||||
|
||||
self.shared_objects = {
|
||||
"screens": {},
|
||||
"regular_classes": {},
|
||||
"threaded_classes": {}
|
||||
}
|
||||
|
||||
# ###### Instantiate Left And Right Screens ######
|
||||
self.shared_objects["screens"]["left_screen"] = \
|
||||
self.create_application_window(UI_FILE_LEFT, "Rover Ground Station Left Screen",
|
||||
self.LEFT_SCREEN_ID) # type: ApplicationWindow
|
||||
|
||||
self.shared_objects["screens"]["right_screen"] = \
|
||||
self.create_application_window(UI_FILE_RIGHT, "Rover Ground Station Right Screen",
|
||||
self.RIGHT_SCREEN_ID) # type: ApplicationWindow
|
||||
|
||||
# ###### Initialize the Ground Station Node ######
|
||||
rospy.init_node("ground_station")
|
||||
|
||||
# ##### Instantiate Regular Classes ######
|
||||
|
||||
# ##### 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("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)))
|
||||
|
||||
def ___ros_master_running(self):
|
||||
checker = ROSMasterChecker.ROSMasterChecker()
|
||||
|
||||
if not checker.master_present(5):
|
||||
self.logger.debug("ROS Master Not Found!!!! Exiting!!!")
|
||||
QtGui.QGuiApplication.exit()
|
||||
return False
|
||||
return True
|
||||
|
||||
def __add_thread(self, thread_name, instance):
|
||||
self.shared_objects["threaded_classes"][thread_name] = instance
|
||||
instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal,
|
||||
self.kill_threads_signal)
|
||||
|
||||
def __connect_signals_to_slots(self):
|
||||
self.shared_objects["screens"]["left_screen"].exit_requested_signal.connect(self.on_exit_requested__slot)
|
||||
self.shared_objects["screens"]["right_screen"].exit_requested_signal.connect(self.on_exit_requested__slot)
|
||||
|
||||
def on_exit_requested__slot(self):
|
||||
self.kill_threads_signal.emit()
|
||||
|
||||
# Wait for Threads
|
||||
for thread in self.shared_objects["threaded_classes"]:
|
||||
self.shared_objects["threaded_classes"][thread].wait()
|
||||
|
||||
QtGui.QGuiApplication.exit()
|
||||
|
||||
@staticmethod
|
||||
def create_application_window(ui_file_path, title, display_screen):
|
||||
system_desktop = QtWidgets.QDesktopWidget() # This gets us access to the desktop geometry
|
||||
|
||||
app_window = ApplicationWindow(parent=None, ui_file_path=ui_file_path) # Make a window in this application
|
||||
app_window.setWindowTitle(title) # Sets the window title
|
||||
|
||||
app_window.setWindowFlags(app_window.windowFlags() | # Sets the windows flags to:
|
||||
QtCore.Qt.FramelessWindowHint | # remove the border and frame on the application,
|
||||
QtCore.Qt.WindowStaysOnTopHint | # and makes the window stay on top of all others
|
||||
QtCore.Qt.X11BypassWindowManagerHint) # This is needed to show fullscreen in gnome
|
||||
|
||||
app_window.setGeometry(
|
||||
system_desktop.screenGeometry(display_screen)) # Sets the window to be on the first screen
|
||||
|
||||
app_window.showFullScreen() # Shows the window in full screen mode
|
||||
|
||||
return app_window
|
||||
|
||||
|
||||
#####################################
|
||||
# Main Definition
|
||||
#####################################
|
||||
if __name__ == "__main__":
|
||||
signal.signal(signal.SIGINT, signal.SIG_DFL) # This allows the keyboard interrupt kill to work properly
|
||||
|
||||
# ########## Start the QApplication Framework ##########
|
||||
application = QtWidgets.QApplication(sys.argv) # Create the ase qt gui application
|
||||
application.setStyleSheet(qdarkstyle.load_stylesheet_pyqt5())
|
||||
|
||||
# ########## Set Organization Details for QSettings ##########
|
||||
QtCore.QCoreApplication.setOrganizationName("OSURC")
|
||||
QtCore.QCoreApplication.setOrganizationDomain("http://osurobotics.club/")
|
||||
QtCore.QCoreApplication.setApplicationName("groundstation")
|
||||
|
||||
# ########## Check ROS Master Status ##########
|
||||
master_checker = ROSMasterChecker.ROSMasterChecker()
|
||||
|
||||
if not master_checker.master_present(5):
|
||||
message_box = QtWidgets.QMessageBox()
|
||||
message_box.setWindowTitle("Rover Ground Station")
|
||||
message_box.setText("Connection to ROS Master Failed!!!\n" +
|
||||
"Ensure ROS master is running or check for network issues.")
|
||||
message_box.exec_()
|
||||
exit()
|
||||
|
||||
# ########## Start Ground Station If Ready ##########
|
||||
ground_station = GroundStation()
|
||||
application.exec_() # Execute launching of the application
|
||||
Reference in New Issue
Block a user