caperren/school_archives
1
0
Fork 0
mirror of https://github.com/caperren/school_archives.git synced 2025-11-09 21:51:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added 2017-2018 mars rover repository.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-22 14:54:52 -07:00
parent a56690ca18
commit 7fd2641766
750 changed files with 2019222 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

197
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/CMakeLists.txt Normal file
View 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
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/package.xml Normal file
View 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>

19
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/scripts/ground_station_launch.sh Executable file
View File

@@ -0,0 +1,19 @@
#!/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
cp ~/key .
sleep 1
export DISPLAY=:0
python ground_station.py

295
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py Normal file
View File

@@ -0,0 +1,295 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets, QtGui
import logging
import rospy
from rover_arm.msg import ArmStatusMessage
from rover_control.msg import GripperStatusMessage
#####################################
# Global Variables
#####################################
ARM_STATUS_TOPIC = "/rover_arm/status"
GRIPPER_STATUS_TOPIC = "/rover_control/gripper/status"
COMMS_TO_STRING = {
0: "NO STATUS",
1: "COMMS OK",
2: "NO DEVICE",
4: "BUS ERROR",
8: "GEN COMM ERROR",
16: "PARAMETER ERROR",
32: "LENGTH ERROR"
}
TARGET_REACHED_BIT_POSITION = 1
STATUS_TO_STRING = {
1: "TARGET REACHED",
2: "ERROR RECOVERY",
3: "RUN",
4: "ENABLED",
5: "FAULT STOP",
6: "WARNING",
7: "STO ACTIVE",
8: "SERVO READY",
10: "BRAKING",
11: "HOMING",
12: "INITIALIZED",
13: "VOLT OK",
15: "PERMANENT STOP"
}
FAULT_TO_STRING = {
1: "TRACKING ERROR",
2: "OVER CURRENT",
# 3: "COMMUNICATION ERROR", # Was showing even though things were working???
4: "ENCODER FAILURE",
5: "OVER TEMP",
6: "UNDER VOLTAGE",
7: "OVER VOLTAGE",
8: "PROG OR MEM ERROR",
9: "HARDWARE ERROR",
10: "OVER VELOCITY",
11: "INIT ERROR",
12: "MOTION ERROR",
13: "RANGE ERROR",
14: "POWER STAGE FORCED OFF",
15: "HOST COMM ERROR"
}
GRIPPER_MODES = {
0: "No Change",
1: "Normal",
2: "Pinch",
3: "Wide ",
4: "Scissor"
}
#####################################
# Controller Class Definition
#####################################
class ArmIndication(QtCore.QObject):
base_position_updated__signal = QtCore.pyqtSignal(float)
shoulder_position_updated__signal = QtCore.pyqtSignal(float)
elbow_position_updated__signal = QtCore.pyqtSignal(float)
roll_position_updated__signal = QtCore.pyqtSignal(float)
wrist_pitch_position_updated__signal = QtCore.pyqtSignal(float)
wrist_roll_position_updated__signal = QtCore.pyqtSignal(float)
base_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
elbow_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
base_status_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_status_update_ready__signal = QtCore.pyqtSignal(str)
elbow_status_update_ready__signal = QtCore.pyqtSignal(str)
roll_status_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_status_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_status_update_ready__signal = QtCore.pyqtSignal(str)
base_faults_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_faults_update_ready__signal = QtCore.pyqtSignal(str)
elbow_faults_update_ready__signal = QtCore.pyqtSignal(str)
roll_faults_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_faults_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_faults_update_ready__signal = QtCore.pyqtSignal(str)
pinch_position_updated__signal = QtCore.pyqtSignal(int)
forefinger_position_updated__signal = QtCore.pyqtSignal(int)
thumb_position_updated__signal = QtCore.pyqtSignal(int)
middlefinger_position_updated__signal = QtCore.pyqtSignal(int)
pinch_current_updated__signal = QtCore.pyqtSignal(int)
forefinger_current_updated__signal = QtCore.pyqtSignal(int)
thumb_current_updated__signal = QtCore.pyqtSignal(int)
middlefinger_current_updated__signal = QtCore.pyqtSignal(int)
gripper_reported_mode_updated__signal = QtCore.pyqtSignal(str)
gripper_reported_setpoint_updated__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
super(ArmIndication, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.right_screen = self.shared_objects["screens"]["right_screen"]
self.base_position_lcd_number = self.right_screen.base_position_lcd_number # type: QtWidgets.QLCDNumber
self.shoulder_position_lcd_number = self.right_screen.shoulder_position_lcd_number # type: QtWidgets.QLCDNumber
self.elbow_position_lcd_number = self.right_screen.elbow_position_lcd_number # type: QtWidgets.QLCDNumber
self.roll_position_lcd_number = self.right_screen.roll_position_lcd_number # type: QtWidgets.QLCDNumber
self.wrist_pitch_position_lcd_number = self.right_screen.wrist_pitch_position_lcd_number # type: QtWidgets.QLCDNumber
self.wrist_roll_position_lcd_number = self.right_screen.wrist_roll_position_lcd_number # type: QtWidgets.QLCDNumber
self.pinch_position_lcd_number = self.right_screen.pinch_position_lcd_number # type: QtWidgets.QLCDNumber
self.forefinger_position_lcd_number = self.right_screen.forefinger_position_lcd_number # type: QtWidgets.QLCDNumber
self.thumb_position_lcd_number = self.right_screen.thumb_position_lcd_number # type: QtWidgets.QLCDNumber
self.middlefinger_position_lcd_number = self.right_screen.middlefinger_position_lcd_number # type: QtWidgets.QLCDNumber
self.pinch_current_lcd_number = self.right_screen.pinch_current_lcd_number # type: QtWidgets.QLCDNumber
self.forefinger_current_lcd_number = self.right_screen.forefinger_current_lcd_number # type: QtWidgets.QLCDNumber
self.thumb_current_lcd_number = self.right_screen.thumb_current_lcd_number # type: QtWidgets.QLCDNumber
self.middlefinger_current_lcd_number = self.right_screen.middlefinger_current_lcd_number # type: QtWidgets.QLCDNumber
self.arm_controls_base_comms_label = self.right_screen.arm_controls_base_comms_label # type:QtWidgets.QLabel
self.arm_controls_base_status_label = self.right_screen.arm_controls_base_status_label # type:QtWidgets.QLabel
self.arm_controls_base_faults_label = self.right_screen.arm_controls_base_faults_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_comms_label = self.right_screen.arm_controls_shoulder_comms_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_status_label = self.right_screen.arm_controls_shoulder_status_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_faults_label = self.right_screen.arm_controls_shoulder_faults_label # type:QtWidgets.QLabel
self.arm_controls_elbow_comms_label = self.right_screen.arm_controls_elbow_comms_label # type:QtWidgets.QLabel
self.arm_controls_elbow_status_label = self.right_screen.arm_controls_elbow_status_label # type:QtWidgets.QLabel
self.arm_controls_elbow_faults_label = self.right_screen.arm_controls_elbow_faults_label # type:QtWidgets.QLabel
self.arm_controls_roll_comms_label = self.right_screen.arm_controls_roll_comms_label # type:QtWidgets.QLabel
self.arm_controls_roll_status_label = self.right_screen.arm_controls_roll_status_label # type:QtWidgets.QLabel
self.arm_controls_roll_faults_label = self.right_screen.arm_controls_roll_faults_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_comms_label = self.right_screen.arm_controls_wrist_pitch_comms_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_status_label = self.right_screen.arm_controls_wrist_pitch_status_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_faults_label = self.right_screen.arm_controls_wrist_pitch_faults_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_comms_label = self.right_screen.arm_controls_wrist_roll_comms_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_status_label = self.right_screen.arm_controls_wrist_roll_status_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_faults_label = self.right_screen.arm_controls_wrist_roll_faults_label # type:QtWidgets.QLabel
self.gripper_reported_mode_label = self.right_screen.gripper_reported_mode_label # type:QtWidgets.QLabel
self.gripper_reported_setpoint_lcd_number = self.right_screen.gripper_reported_setpoint_lcd_number # type: QtWidgets.QLCDNumber
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
# ########## Get the Pick And Plate instance of the logger ##########
self.logger = logging.getLogger("groundstation")
# ########## Class Variables ##########
self.arm_status_subscriber = rospy.Subscriber(ARM_STATUS_TOPIC, ArmStatusMessage, self.on_arm_status_update_received__callback)
self.gripper_status_subscriver = rospy.Subscriber(GRIPPER_STATUS_TOPIC, GripperStatusMessage, self.on_gripper_status_update_received__callback)
# ########## Connect Signals and Slots ##########
self.connect_signals_and_slots()
def connect_signals_and_slots(self):
self.base_position_updated__signal.connect(self.base_position_lcd_number.display)
self.shoulder_position_updated__signal.connect(self.shoulder_position_lcd_number.display)
self.elbow_position_updated__signal.connect(self.elbow_position_lcd_number.display)
self.roll_position_updated__signal.connect(self.roll_position_lcd_number.display)
self.wrist_pitch_position_updated__signal.connect(self.wrist_pitch_position_lcd_number.display)
self.wrist_roll_position_updated__signal.connect(self.wrist_roll_position_lcd_number.display)
self.base_comms_state_update_ready__signal.connect(self.arm_controls_base_comms_label.setText)
self.shoulder_comms_state_update_ready__signal.connect(self.arm_controls_shoulder_comms_label.setText)
self.elbow_comms_state_update_ready__signal.connect(self.arm_controls_elbow_comms_label.setText)
self.roll_comms_state_update_ready__signal.connect(self.arm_controls_roll_comms_label.setText)
self.wrist_pitch_comms_state_update_ready__signal.connect(self.arm_controls_wrist_pitch_comms_label.setText)
self.wrist_roll_comms_state_update_ready__signal.connect(self.arm_controls_wrist_roll_comms_label.setText)
self.base_status_update_ready__signal.connect(self.arm_controls_base_status_label.setText)
self.shoulder_status_update_ready__signal.connect(self.arm_controls_shoulder_status_label.setText)
self.elbow_status_update_ready__signal.connect(self.arm_controls_elbow_status_label.setText)
self.roll_status_update_ready__signal.connect(self.arm_controls_roll_status_label.setText)
self.wrist_pitch_status_update_ready__signal.connect(self.arm_controls_wrist_pitch_status_label.setText)
self.wrist_roll_status_update_ready__signal.connect(self.arm_controls_wrist_roll_status_label.setText)
self.base_faults_update_ready__signal.connect(self.arm_controls_base_faults_label.setText)
self.shoulder_faults_update_ready__signal.connect(self.arm_controls_shoulder_faults_label.setText)
self.elbow_faults_update_ready__signal.connect(self.arm_controls_elbow_faults_label.setText)
self.roll_faults_update_ready__signal.connect(self.arm_controls_roll_faults_label.setText)
self.wrist_pitch_faults_update_ready__signal.connect(self.arm_controls_wrist_pitch_faults_label.setText)
self.wrist_roll_faults_update_ready__signal.connect(self.arm_controls_wrist_roll_faults_label.setText)
self.pinch_position_updated__signal.connect(self.pinch_position_lcd_number.display)
self.forefinger_position_updated__signal.connect(self.forefinger_position_lcd_number.display)
self.thumb_position_updated__signal.connect(self.thumb_position_lcd_number.display)
self.middlefinger_position_updated__signal.connect(self.middlefinger_position_lcd_number.display)
self.pinch_current_updated__signal.connect(self.pinch_current_lcd_number.display)
self.forefinger_current_updated__signal.connect(self.forefinger_current_lcd_number.display)
self.thumb_current_updated__signal.connect(self.thumb_current_lcd_number.display)
self.middlefinger_current_updated__signal.connect(self.middlefinger_current_lcd_number.display)
self.gripper_reported_mode_updated__signal.connect(self.gripper_reported_mode_label.setText)
self.gripper_reported_setpoint_updated__signal.connect(self.gripper_reported_setpoint_lcd_number.display)
def on_arm_status_update_received__callback(self, data):
self.base_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.base_comm_status))
self.shoulder_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.shoulder_comm_status))
self.elbow_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.elbow_comm_status))
self.roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.roll_comm_status))
self.wrist_pitch_comms_state_update_ready__signal.emit(
self.process_comms_to_string(data.wrist_pitch_comm_status))
self.wrist_roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.wrist_roll_comm_status))
self.base_status_update_ready__signal.emit(self.process_statuses_to_string(data.base_status))
self.shoulder_status_update_ready__signal.emit(self.process_statuses_to_string(data.shoulder_status))
self.elbow_status_update_ready__signal.emit(self.process_statuses_to_string(data.elbow_status))
self.roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.roll_status))
self.wrist_pitch_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_pitch_status))
self.wrist_roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_roll_status))
self.base_faults_update_ready__signal.emit(self.process_faults_to_string(data.base_faults))
self.shoulder_faults_update_ready__signal.emit(self.process_faults_to_string(data.shoulder_faults))
self.elbow_faults_update_ready__signal.emit(self.process_faults_to_string(data.elbow_faults))
self.roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.roll_faults))
self.wrist_pitch_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_pitch_faults))
self.wrist_roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_roll_faults))
self.base_position_updated__signal.emit(data.base)
self.shoulder_position_updated__signal.emit(data.shoulder)
self.elbow_position_updated__signal.emit(data.elbow)
self.roll_position_updated__signal.emit(data.roll)
self.wrist_pitch_position_updated__signal.emit(data.wrist_pitch)
self.wrist_roll_position_updated__signal.emit(data.wrist_roll)
def on_gripper_status_update_received__callback(self, data):
data = data # type: GripperStatusMessage
self.pinch_position_updated__signal.emit(data.pinch_position_raw)
self.forefinger_position_updated__signal.emit(data.forefinger_position_raw)
self.thumb_position_updated__signal.emit(data.thumb_position_raw)
self.middlefinger_position_updated__signal.emit(data.middlefinger_position_raw)
self.pinch_current_updated__signal.emit(data.pinch_current)
self.forefinger_current_updated__signal.emit(data.forefinger_current)
self.thumb_current_updated__signal.emit(data.thumb_current)
self.middlefinger_current_updated__signal.emit(data.middlefinger_current)
self.gripper_reported_mode_updated__signal.emit(GRIPPER_MODES[data.current_mode])
self.gripper_reported_setpoint_updated__signal.emit(data.current_finger_position)
@staticmethod
def process_faults_to_string(faults):
fault_output = ""
for bit_position in FAULT_TO_STRING:
if (1 << bit_position) & faults:
fault_output += FAULT_TO_STRING[bit_position] + "\n"
return fault_output[:-1]
@staticmethod
def process_statuses_to_string(statuses):
status_output = ""
for bit_position in STATUS_TO_STRING:
if (1 << bit_position) & statuses:
status_output += STATUS_TO_STRING[bit_position] + "\n"
return status_output[:-1]
@staticmethod
def process_comms_to_string(comms):
return COMMS_TO_STRING[comms] if comms in COMMS_TO_STRING else "UNKNOWN"

0
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/ArmSystems/__init__.py Normal file
View File

337
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/ControlSystems/DriveAndCameraControlSender.py Normal file
View File

@@ -0,0 +1,337 @@
#####################################
# 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, TowerPanTiltControlMessage
#####################################
# Global Variables
#####################################
GAME_CONTROLLER_NAME = "Microsoft X-Box One S pad"
DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive"
DEFAULT_TOWER_PAN_TILT_COMMAND_TOPIC = "/rover_control/tower/pan_tilt/control"
DEFAULT_CHASSIS_PAN_TILT_COMMAND_TOPIC = "/rover_control/chassis/pan_tilt/control"
DRIVE_COMMAND_HERTZ = 20
STICK_DEADBAND = 3500
STICK_MAX = 32768.0
STICK_OFFSET = 0
THROTTLE_MIN = 0.05
PAUSE_STATE_CHANGE_TIME = 0.5
CAMERA_CHANGE_TIME = 0.2
GUI_ELEMENT_CHANGE_TIME = 0.2
CAMERA_TOGGLE_CHANGE_TIME = 0.35
TOWER_PAN_TILT_X_AXIS_SCALAR = 2
TOWER_PAN_TILT_Y_AXIS_SCALAR = 15
CHASSIS_PAN_TILT_X_AXIS_SCALAR = 15
CHASSIS_PAN_TILT_Y_AXIS_SCALAR = 15
#####################################
# 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_x_axis": 0,
"left_y_axis": 0,
"right_x_axis": 0,
"right_y_axis": 0,
"left_trigger": 0,
"right_trigger": 0,
"left_stick": 0,
"right_right": 0,
"left_bumper": 0,
"right_bumper": 0,
"d_pad_x": 0,
"d_pad_y": 0,
"back": 0,
"start": 0,
"a": 0,
"x": 0,
"y": 0,
"b": 0,
}
self.raw_mapping_to_class_mapping = {
"ABS_X": "left_x_axis",
"ABS_Y": "left_y_axis",
"ABS_RX": "right_x_axis",
"ABS_RY": "right_y_axis",
"ABS_Z": "left_trigger",
"ABS_RZ": "right_trigger",
"BTN_THUMBL": "left_stick",
"BTN_THUMBR": "right_right",
"BTN_TL": "left_bumper",
"BTN_TR": "right_bumper",
"ABS_HAT0X": "d_pad_x",
"ABS_HAT0Y": "d_pad_y",
"BTN_SELECT": "back",
"BTN_START": "start",
"BTN_SOUTH": "a",
"BTN_NORTH": "x",
"BTN_WEST": "y",
"BTN_EAST": "b",
}
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:
# print event.code, event.state
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
#####################################
# Controller Class Definition
#####################################
class DriveAndCameraControlSender(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)
change_gui_element_selection__signal = QtCore.pyqtSignal(int)
change_camera_selection__signal = QtCore.pyqtSignal(int)
toggle_selected_gui_camera__signal = QtCore.pyqtSignal()
def __init__(self, shared_objects):
super(DriveAndCameraControlSender, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.video_coordinator = self.shared_objects["threaded_classes"]["Video Coordinator"]
self.right_screen = self.shared_objects["screens"]["right_screen"]
self.rover_speed_limit_slider = self.right_screen.rover_speed_limit_slider # type: QtWidgets.QSlider
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()
# ########## 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.tower_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_COMMAND_TOPIC,
TowerPanTiltControlMessage, queue_size=1)
self.chassis_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_CHASSIS_PAN_TILT_COMMAND_TOPIC,
TowerPanTiltControlMessage, queue_size=1)
self.current_pan_tilt_selection = "no_pan_tilt"
self.last_hat_x_was_movement = False
self.last_hat_y_was_movement = False
self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
self.drive_paused = True
self.last_pause_state_time = time()
self.last_gui_element_change_time = time()
self.last_camera_change_time = time()
self.last_camera_toggle_time = time()
self.speed_limit = 0.5
def run(self):
self.logger.debug("Starting Joystick Thread")
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(int(self.wait_time - time_diff), 0))
self.logger.debug("Stopping Joystick Thread")
def connect_signals_and_slots(self):
self.set_left_drive_output__signal.connect(self.left_drive_progress_bar.setValue)
self.set_right_drive_output__signal.connect(self.right_drive_progress_bar.setValue)
self.video_coordinator.pan_tilt_selection_changed__signal.connect(self.on_pan_tilt_selection_changed__slot)
self.rover_speed_limit_slider.valueChanged.connect(self.on_speed_limit_slider_value_changed__slot)
def check_and_set_pause_state(self):
thumb_pressed = self.joystick.controller_states["start"]
if thumb_pressed and (time() - self.last_pause_state_time) > PAUSE_STATE_CHANGE_TIME:
self.drive_paused = not self.drive_paused
self.show_changed_pause_state()
self.last_pause_state_time = time()
def __update_and_publish(self):
self.publish_drive_command()
self.publish_camera_control_commands()
self.publish_pan_tilt_control_commands()
def publish_drive_command(self):
if self.drive_paused:
drive_message = DriveCommandMessage()
else:
drive_message = self.get_drive_message(self.speed_limit)
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(self.speed_limit * 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 publish_camera_control_commands(self):
trigger_pressed = self.joystick.controller_states["y"]
three_pressed = self.joystick.controller_states["left_bumper"]
four_pressed = self.joystick.controller_states["right_bumper"]
five_pressed = self.joystick.controller_states["left_trigger"]
six_pressed = self.joystick.controller_states["right_trigger"]
if (five_pressed or six_pressed) and (time() - self.last_camera_change_time) > CAMERA_CHANGE_TIME:
change = -1 if five_pressed else 1
self.change_camera_selection__signal.emit(change)
self.last_camera_change_time = time()
if (three_pressed or four_pressed) and (time() - self.last_gui_element_change_time) > GUI_ELEMENT_CHANGE_TIME:
change = -1 if three_pressed else 1
self.change_gui_element_selection__signal.emit(change)
self.last_gui_element_change_time = time()
if trigger_pressed and (time() - self.last_camera_toggle_time) > CAMERA_TOGGLE_CHANGE_TIME:
self.toggle_selected_gui_camera__signal.emit()
self.last_camera_toggle_time = time()
def publish_pan_tilt_control_commands(self):
button_eight = self.joystick.controller_states["a"]
hat_x = self.joystick.controller_states["d_pad_x"]
hat_y = self.joystick.controller_states["d_pad_y"]
if (hat_x == 0 and not self.last_hat_x_was_movement) and (
hat_y == 0 and not self.last_hat_y_was_movement) and not button_eight:
return
self.last_hat_x_was_movement = True if hat_x != 0 else False
self.last_hat_y_was_movement = True if hat_y != 0 else False
pan_tilt_message = TowerPanTiltControlMessage()
if button_eight:
pan_tilt_message.should_center = 1
if self.current_pan_tilt_selection == "tower_pan_tilt":
pan_tilt_message.relative_pan_adjustment = hat_x * TOWER_PAN_TILT_X_AXIS_SCALAR
pan_tilt_message.relative_tilt_adjustment = -(hat_y * TOWER_PAN_TILT_Y_AXIS_SCALAR)
self.tower_pan_tilt_command_publisher.publish(pan_tilt_message)
elif self.current_pan_tilt_selection == "chassis_pan_tilt":
pan_tilt_message.relative_pan_adjustment = hat_x * CHASSIS_PAN_TILT_X_AXIS_SCALAR
pan_tilt_message.relative_tilt_adjustment = -(hat_y * CHASSIS_PAN_TILT_Y_AXIS_SCALAR)
self.chassis_pan_tilt_command_publisher.publish(pan_tilt_message)
def get_drive_message(self, speed_limit):
drive_message = DriveCommandMessage()
left_y_axis = self.joystick.controller_states["left_y_axis"] if abs(self.joystick.controller_states["left_y_axis"]) > STICK_DEADBAND else 0
right_y_axis = self.joystick.controller_states["right_y_axis"] if abs(self.joystick.controller_states["right_y_axis"]) > STICK_DEADBAND else 0
left_y_axis = speed_limit * (-(left_y_axis - STICK_OFFSET) / STICK_MAX)
right_y_axis = speed_limit * (-(right_y_axis - STICK_OFFSET) / STICK_MAX)
drive_message.drive_twist.linear.x = (left_y_axis + right_y_axis) / 2.0
drive_message.drive_twist.angular.z = (right_y_axis - left_y_axis) / 2.0
return drive_message
def on_pan_tilt_selection_changed__slot(self, selection):
self.current_pan_tilt_selection = selection
def on_speed_limit_slider_value_changed__slot(self, value):
self.speed_limit = value / 100.0
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

445
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/ControlSystems/EffectorsAndArmControlSender.py Normal file
View File

@@ -0,0 +1,445 @@
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
from inputs import devices, GamePad
from time import time
import rospy
from rover_arm.msg import ArmControlMessage
from rover_control.msg import MiningControlMessage, GripperControlMessage
#####################################
# Global Variables
#####################################
GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360"
DRIVE_COMMAND_HERTZ = 20
GRIPPER_CONTROL_TOPIC = "/rover_control/gripper/control"
RELATIVE_ARM_CONTROL_TOPIC = "/rover_arm/control/relative"
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
BASE_SCALAR = 0.003
SHOULDER_SCALAR = 0.002
ELBOW_SCALAR = 0.002
ROLL_SCALAR = 0.003
WRIST_PITCH_SCALAR = 0.003
WRIST_ROLL_SCALAR = 0.006
GRIPPER_MOVEMENT_SCALAR = 1500
LEFT_X_AXIS_DEADZONE = 1500
LEFT_Y_AXIS_DEADZONE = 1500
RIGHT_X_AXIS_DEADZONE = 1500
RIGHT_Y_AXIS_DEADZONE = 1500
THUMB_STICK_MAX = 32768.0
MINING_LIFT_SCALAR = 5
MINING_TILT_SCALAR = 5
COLOR_GREEN = "background-color:darkgreen; border: 1px solid black;"
COLOR_NONE = "border: 1px solid black;"
#####################################
# Controller Class Definition
#####################################
class XBOXController(QtCore.QThread):
def __init__(self):
super(XBOXController, 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_x_axis": 0,
"left_y_axis": 0,
"left_stick_button": 0,
"right_x_axis": 0,
"right_y_axis": 0,
"right_stick_button": 0,
"left_trigger": 0,
"left_bumper": 0,
"right_trigger": 0,
"right_bumper": 0,
"hat_x_axis": 0,
"hat_y_axis": 0,
"back_button": 0,
"start_button": 0,
"xbox_button": 0,
"x_button": 0,
"a_button": 0,
"b_button": 0,
"y_button": 0
}
self.raw_mapping_to_class_mapping = {
"ABS_X": "left_x_axis",
"ABS_Y": "left_y_axis",
"BTN_THUMBL": "left_stick_button",
"ABS_RX": "right_x_axis",
"ABS_RY": "right_y_axis",
"BTN_THUMBR": "right_stick_button",
"ABS_Z": "left_trigger",
"BTN_TL": "left_bumper",
"ABS_RZ": "right_trigger",
"BTN_TR": "right_bumper",
"ABS_HAT0X": "hat_x_axis",
"ABS_HAT0Y": "hat_y_axis",
"BTN_SELECT": "back_button",
"BTN_START": "start_button",
"BTN_MODE": "xbox_button",
"BTN_NORTH": "x_button",
"BTN_SOUTH": "a_button",
"BTN_EAST": "b_button",
"BTN_WEST": "y_button"
}
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:
# For seeing codes you haven't added yet...
# if event.code not in self.raw_mapping_to_class_mapping and event.code != "SYN_REPORT":
# print event.code, ":", event.state
if event.code in self.raw_mapping_to_class_mapping:
# print event.code, ":", event.state
self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
self.ready = True
#####################################
# Controller Class Definition
#####################################
class EffectorsAndArmControlSender(QtCore.QThread):
xbox_control_arm_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
xbox_control_mining_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
gripper_mode_normal_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
gripper_mode_pinch_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
gripper_mode_wide_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
gripper_mode_scissor_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
XBOX_CONTROL_STATES = [
"ARM",
"MINING"
]
GRIPPER_CONTROL_MODES = {
"NORMAL": 1,
"TWO_FINGER_PINCH": 2,
"WIDE": 3,
"SCISSOR": 4
}
PINCH_MODE_ABSOLUTE_SET_POSITION = 57740
def __init__(self, shared_objects):
super(EffectorsAndArmControlSender, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.right_screen = self.shared_objects["screens"]["right_screen"]
self.xbox_mode_arm_label = self.right_screen.xbox_mode_arm_label # type: QtWidgets.QLabel
self.xbox_mode_mining_label = self.right_screen.xbox_mode_mining_label # type: QtWidgets.QLabel
self.gripper_mode_normal_label = self.right_screen.gripper_mode_normal_label # type: QtWidgets.QLabel
self.gripper_mode_pinch_label = self.right_screen.gripper_mode_pinch_label # type: QtWidgets.QLabel
self.gripper_mode_wide_label = self.right_screen.gripper_mode_wide_label # type: QtWidgets.QLabel
self.gripper_mode_scissor_label = self.right_screen.gripper_mode_scissor_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.controller = XBOXController()
# ########## Class Variables ##########
self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
self.gripper_control_publisher = rospy.Publisher(GRIPPER_CONTROL_TOPIC, GripperControlMessage, queue_size=1)
self.relative_arm_control_publisher = rospy.Publisher(RELATIVE_ARM_CONTROL_TOPIC, ArmControlMessage,
queue_size=1)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.xbox_current_control_state = self.XBOX_CONTROL_STATES.index("ARM")
self.xbox_control_state_just_changed = False
self.last_xbox_button_state = 0
self.last_left_bumper_state = 0
self.last_right_bumper_state = 0
self.last_back_button_state = 0
self.gripper_control_mode = 1
self.gripper_control_mode_just_changed = False
self.send_new_gripper_mode = False
self.GRIPPER_DISPLAY_SIGNAL_MAPPING = {
self.GRIPPER_CONTROL_MODES["NORMAL"]: {
"SET": [self.gripper_mode_normal_stylesheet_update_ready__signal],
"UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal,
self.gripper_mode_wide_stylesheet_update_ready__signal,
self.gripper_mode_scissor_stylesheet_update_ready__signal]
},
self.GRIPPER_CONTROL_MODES["TWO_FINGER_PINCH"]: {
"SET": [self.gripper_mode_pinch_stylesheet_update_ready__signal],
"UNSET": [self.gripper_mode_normal_stylesheet_update_ready__signal,
self.gripper_mode_wide_stylesheet_update_ready__signal,
self.gripper_mode_scissor_stylesheet_update_ready__signal]
},
self.GRIPPER_CONTROL_MODES["WIDE"]: {
"SET": [self.gripper_mode_wide_stylesheet_update_ready__signal],
"UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal,
self.gripper_mode_normal_stylesheet_update_ready__signal,
self.gripper_mode_scissor_stylesheet_update_ready__signal]
},
self.GRIPPER_CONTROL_MODES["SCISSOR"]: {
"SET": [self.gripper_mode_scissor_stylesheet_update_ready__signal],
"UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal,
self.gripper_mode_wide_stylesheet_update_ready__signal,
self.gripper_mode_normal_stylesheet_update_ready__signal],
# "ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION
}
}
def run(self):
self.logger.debug("Starting Joystick Thread")
while self.run_thread_flag:
start_time = time()
self.change_control_state_if_needed()
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
self.send_gripper_home_on_back_press()
self.process_and_send_arm_control()
elif self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("MINING"):
self.send_mining_commands()
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
self.logger.debug("Stopping Joystick Thread")
def connect_signals_and_slots(self):
self.xbox_control_arm_stylesheet_update_ready__signal.connect(self.xbox_mode_arm_label.setStyleSheet)
self.xbox_control_mining_stylesheet_update_ready__signal.connect(self.xbox_mode_mining_label.setStyleSheet)
self.gripper_mode_normal_stylesheet_update_ready__signal.connect(self.gripper_mode_normal_label.setStyleSheet)
self.gripper_mode_pinch_stylesheet_update_ready__signal.connect(self.gripper_mode_pinch_label.setStyleSheet)
self.gripper_mode_wide_stylesheet_update_ready__signal.connect(self.gripper_mode_wide_label.setStyleSheet)
self.gripper_mode_scissor_stylesheet_update_ready__signal.connect(self.gripper_mode_scissor_label.setStyleSheet)
def change_control_state_if_needed(self):
xbox_state = self.controller.controller_states["xbox_button"]
left_bumper_state = self.controller.controller_states["left_bumper"]
right_bumper_state = self.controller.controller_states["right_bumper"]
if self.last_xbox_button_state == 0 and xbox_state == 1:
self.xbox_current_control_state += 1
self.xbox_current_control_state = self.xbox_current_control_state % len(self.XBOX_CONTROL_STATES)
self.xbox_control_state_just_changed = True
self.last_xbox_button_state = 1
elif self.last_xbox_button_state == 1 and xbox_state == 0:
self.last_xbox_button_state = 0
if self.xbox_control_state_just_changed:
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_NONE)
elif self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("MINING"):
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_state_just_changed = False
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
if self.last_left_bumper_state == 0 and left_bumper_state == 1:
# self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode_just_changed = True
self.last_left_bumper_state = 1
elif self.last_left_bumper_state == 1 and left_bumper_state == 0:
self.last_left_bumper_state = 0
if self.last_right_bumper_state == 0 and right_bumper_state == 1:
# self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode_just_changed = True
self.last_right_bumper_state = 1
elif self.last_right_bumper_state == 1 and right_bumper_state == 0:
self.last_right_bumper_state = 0
if self.gripper_control_mode_just_changed:
signal_map = self.GRIPPER_DISPLAY_SIGNAL_MAPPING[self.gripper_control_mode + 1]
for signal in signal_map["SET"]:
signal.emit(COLOR_GREEN)
for signal in signal_map["UNSET"]:
signal.emit(COLOR_NONE)
if "ABS_MOVE" in signal_map:
gripper_control_message = GripperControlMessage()
gripper_control_message.gripper_mode = self.GRIPPER_CONTROL_MODES["SCISSOR"]
gripper_control_message.gripper_position_absolute = signal_map["ABS_MOVE"]
self.gripper_control_publisher.publish(gripper_control_message)
else:
self.send_new_gripper_mode = True
self.gripper_control_mode_just_changed = False
def process_and_send_arm_control(self):
arm_control_message = ArmControlMessage()
gripper_control_message = GripperControlMessage()
should_publish_arm = False
should_publish_gripper = True if self.send_new_gripper_mode else False
if self.send_new_gripper_mode:
gripper_control_message.gripper_position_absolute = 0
else:
gripper_control_message.gripper_position_absolute = -1
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
left_trigger = self.controller.controller_states["left_trigger"]
right_trigger = self.controller.controller_states["right_trigger"]
left_x_axis = self.controller.controller_states["left_x_axis"] if abs(self.controller.controller_states[
"left_x_axis"]) > LEFT_X_AXIS_DEADZONE else 0
left_y_axis = self.controller.controller_states["left_y_axis"] if abs(self.controller.controller_states[
"left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0
right_y_axis = self.controller.controller_states["right_y_axis"] if abs(self.controller.controller_states[
"right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0
right_x_axis = self.controller.controller_states["right_x_axis"] if abs(self.controller.controller_states[
"right_x_axis"]) > RIGHT_X_AXIS_DEADZONE else 0
# print left_x_axis, ":", left_y_axis, ":", right_x_axis, ":", right_y_axis
left_trigger_ratio = left_trigger / 255.0
right_trigger_ratio = right_trigger / 255.0
if left_trigger > 0:
should_publish_arm = True
arm_control_message.base = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * left_trigger_ratio
arm_control_message.shoulder = ((left_y_axis / THUMB_STICK_MAX) * SHOULDER_SCALAR) * left_trigger_ratio
arm_control_message.elbow = (-(right_y_axis / THUMB_STICK_MAX) * ELBOW_SCALAR) * left_trigger_ratio
arm_control_message.roll = (-(right_x_axis / THUMB_STICK_MAX) * ROLL_SCALAR) * left_trigger_ratio
elif right_trigger > 0:
should_publish_arm = True
should_publish_gripper = True
arm_control_message.wrist_roll = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * right_trigger_ratio
# ##### FIXME #####
# Remove this once the arm is fixed
# arm_control_message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio
# #################
gripper_control_message.gripper_position_relative = (-(right_y_axis / THUMB_STICK_MAX) * GRIPPER_MOVEMENT_SCALAR) * right_trigger_ratio
if should_publish_arm:
self.relative_arm_control_publisher.publish(arm_control_message)
if should_publish_gripper:
self.gripper_control_publisher.publish(gripper_control_message)
self.send_new_gripper_mode = False
def send_gripper_home_on_back_press(self):
gripper_control_message = GripperControlMessage()
back_state = self.controller.controller_states["back_button"]
if self.last_back_button_state == 0 and back_state == 1:
gripper_control_message.should_home = True
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
self.gripper_control_publisher.publish(gripper_control_message)
self.last_back_button_state = 1
elif self.last_back_button_state == 1 and back_state == 0:
self.last_back_button_state = 0
def send_mining_commands(self):
left_y_axis = self.controller.controller_states["left_y_axis"] if abs(
self.controller.controller_states["left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0
right_y_axis = self.controller.controller_states["right_y_axis"] if abs(self.controller.controller_states[
"right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0
if left_y_axis or right_y_axis:
message = MiningControlMessage()
message.lift_set_absolute = 1024
message.tilt_set_absolute = 1024
message.lift_set_relative = (-(left_y_axis / THUMB_STICK_MAX) * MINING_LIFT_SCALAR)
message.tilt_set_relative = ((right_y_axis / THUMB_STICK_MAX) * MINING_TILT_SCALAR)
message.cal_factor = -1
self.mining_control_publisher.publish(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 on_kill_threads_requested__slot(self):
self.run_thread_flag = False

175
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/ControlSystems/SpaceNavControlSender.py Normal file
View File

@@ -0,0 +1,175 @@
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore
import logging
from time import time
import spnav
import rospy
from rover_control.msg import MiningControlMessage
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 100
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
Y_ANGULAR_DEADBAND = 0.05
Z_LINEAR_DEADBAND = 0.15
MINING_LIFT_SCALAR = 5
MINING_TILT_SCALAR = 5
#####################################
# Controller Class Definition
#####################################
class SpaceNavControlSender(QtCore.QThread):
spacenav_state_update__signal = QtCore.pyqtSignal(object)
MINING_MODE = 0
ARM_MODE = 1
def __init__(self, shared_objects):
super(SpaceNavControlSender, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.right_screen = self.shared_objects["screens"]["left_screen"]
# ########## 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.wait_time = 1.0 / THREAD_HERTZ
self.spnav_states = {
"linear_x": 0,
"linear_y": 0,
"linear_z": 0,
"angular_x": 0,
"angular_y": 0,
"angular_z": 0,
"panel_pressed": 0,
"fit_pressed": 0,
"shift_pressed": 0,
"alt_pressed": 0,
"ctrl_pressed": 0,
"esc_pressed": 0,
"1_pressed": 0,
"2_pressed": 0,
"minus_pressed": 0,
"plus_pressed": 0,
"t_pressed": 0,
"l_pressed": 0,
"2d_pressed": 0,
"r_pressed": 0,
"f_pressed": 0
}
self.event_mapping_to_button_mapping = {
11: "panel_pressed",
10: "fit_pressed",
8: "shift_pressed",
7: "alt_pressed",
9: "ctrl_pressed",
6: "esc_pressed",
0: "1_pressed",
1: "2_pressed",
13: "minus_pressed",
12: "plus_pressed",
2: "t_pressed",
3: "l_pressed",
14: "2d_pressed",
4: "r_pressed",
5: "f_pressed"
}
# ##### Mining Control #####
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.current_control_mode = self.MINING_MODE
def run(self):
self.logger.debug("Starting SpaceNav Mouse Thread")
spnav.spnav_open()
while self.run_thread_flag:
start_time = time()
self.process_spnav_events()
self.check_control_mode_change()
self.broadcast_control_state()
time_diff = time() - start_time
# self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
self.logger.debug("Stopping SpaceNav Mouse Thread")
def process_spnav_events(self):
event = spnav.spnav_poll_event()
if event:
# print event
if event.ev_type == spnav.SPNAV_EVENT_MOTION:
self.spnav_states["linear_x"] = event.translation[0] / 350.0
self.spnav_states["linear_y"] = event.translation[2] / 350.0
self.spnav_states["linear_z"] = event.translation[1] / 350.0
self.spnav_states["angular_x"] = event.rotation[2] / 350.0
self.spnav_states["angular_y"] = -(event.rotation[0] / 350.0)
self.spnav_states["angular_z"] = -(event.rotation[1] / 350.0)
# print "x", self.spnav_states["linear_x"], "\t", "y", self.spnav_states["linear_y"], "\t", "z", self.spnav_states["linear_z"]
# print "x", self.spnav_states["angular_x"], "\t", "y", self.spnav_states["angular_y"], "\t", "z", self.spnav_states["angular_z"]
else:
self.spnav_states[self.event_mapping_to_button_mapping[event.bnum]] = event.press
def check_control_mode_change(self):
if self.spnav_states["1_pressed"]:
self.current_control_mode = self.MINING_MODE
elif self.spnav_states["2_pressed"]:
self.current_control_mode = self.ARM_MODE
def broadcast_control_state(self):
if self.current_control_mode == self.MINING_MODE:
self.send_mining_commands()
# self.spacenav_state_update__signal.emit(self.spnav_states)
elif self.current_control_mode == self.ARM_MODE:
pass
# print self.spnav_states["linear_z"], self.spnav_states["angular_y"]
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

0
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/ControlSystems/__init__.py Normal file
View File

127
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/LoggingSystems/Logger.py Normal file
View File

@@ -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
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/LoggingSystems/__init__.py Normal file
View File

480
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py Normal file
View File

@@ -0,0 +1,480 @@
'''
MappingSettings.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 PIL.Image
import PIL.ImageFont
import signing
import RoverMapHelper as MapHelper
import cv2
import numpy as np
#####################################
# 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 = 6
_PRECISION_FORMAT = '%.' + str(_DEGREE_PRECISION) + 'f'
# 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=' + _PRECISION_FORMAT + ',' + _PRECISION_FORMAT + '&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/' + ((_PRECISION_FORMAT + '_' + _PRECISION_FORMAT + '_%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])
try:
result = urllib2.urlopen(urllib2.Request(url)).read()
except urllib2.HTTPError, e:
print "Error accessing url for reason:", e
print url
return
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.big_image_copy = None
self.display_image = None
self.display_image_copy = None
self.indicator = 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))
self.generate_image_files()
self.write_once = True
# Text Drawing Variables
self.font = cv2.FONT_HERSHEY_TRIPLEX
self.font_thickness = 1
self.font_baseline = 0
self.nav_coordinates_text_image = None
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.big_image_copy = self.big_image.copy()
self.display_image = self.helper.new_image(self.width, self.height,
True)
self.display_image_copy = self.display_image.copy()
self.load_rover_icon()
self.indicator.save("location.png")
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
pixel_per_lat = self.big_height / viewport_lat_diff
pixel_per_lon = self.big_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, navigation_list, landmark_list):
self.big_image = self.big_image_copy.copy()
self.display_image = self.display_image_copy.copy()
size = 5
draw = PIL.ImageDraw.Draw(self.big_image)
for element in navigation_list:
x, y = self._get_cartesian(float(element[1]), float(element[2]))
draw.text((x + 10, y - 5), str(element[0]))
draw.ellipse((x - size, y - size, x + size, y + size), fill=(element[3].red(), element[3].green(), element[3].blue()))
for element in landmark_list:
x, y = self._get_cartesian(element[1], element[2])
draw.text((x + 10, y - 5), str(element[0]))
draw.ellipse((x - size, y - size, x + size, y + size), fill=(element[3].red(), element[3].green(), element[3].blue()))
self._draw_rover(latitude, longitude, compass)
self.update(latitude, longitude)
return self.display_image
def load_rover_icon(self):
self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((40, 40))
def _draw_rover(self, lat, lon, angle=0):
x, y = self._get_cartesian(lat, lon)
x -= 25 # Half the height of the icon
y -= 25
rotated = self.indicator.copy()
rotated = rotated.rotate(-angle, resample=PIL.Image.BICUBIC)
# rotated.save("rotated.png")
self.big_image.paste(rotated, (x, y), rotated)
if self.write_once:
# self.display_image.save("Something.png")
self.write_once = False
def update(self, latitude, longitude):
# self.left_x -= 50
# self.upper_y -= 50
self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
# self._draw_coordinate_text(latitude, longitude)
def connect_signals_and_slots(self):
pass

375
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py Normal file
View File

@@ -0,0 +1,375 @@
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets, QtGui
from PIL.ImageQt import ImageQt
from PIL import Image
import PIL.ImageDraw
import PIL.Image
import PIL.ImageFont
import numpy
import logging
import rospy
from tf import transformations
from scipy.interpolate import interp1d
import math
from sensor_msgs.msg import Imu
# Custom Imports
import RoverMap
from Resources.Settings import MappingSettings
from sensor_msgs.msg import NavSatFix
#####################################
# Global Variables
#####################################
# put some stuff here later so you can remember
GPS_POSITION_TOPIC = "/rover_odometry/fix"
IMU_DATA_TOPIC = "/rover_odometry/imu/data"
MAP_WIDGET_WIDTH = float(1280)
MAP_WIDGET_HEIGHT = float(720)
MAP_WIDGET_RATIO = MAP_WIDGET_WIDTH / MAP_WIDGET_HEIGHT
class RoverMapCoordinator(QtCore.QThread):
pixmap_ready_signal = QtCore.pyqtSignal()
change_waypoint_signal = QtCore.pyqtSignal()
zoom_level_enabled_state_update__signal = QtCore.pyqtSignal(int)
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 # type:QtWidgets.QLabel
self.navigation_label = self.left_screen.navigation_waypoints_table_widget
self.landmark_label = self.left_screen.landmark_waypoints_table_widget
self.map_selection_combo_box = self.left_screen.map_selection_combo_box # type: QtWidgets.QComboBox
self.map_zoom_level_combo_box = self.left_screen.map_zoom_level_combo_box # type: QtWidgets.QComboBox
self.setings = QtCore.QSettings()
self.logger = logging.getLogger("groundstation")
self.gps_position_subscriber = rospy.Subscriber(GPS_POSITION_TOPIC, NavSatFix,
self.gps_position_updated_callback)
self.run_thread_flag = True
self.setup_map_flag = True
self.google_maps_object = None
self.map_image = None
self.map_image_copy = None
self.overlay_image = None
self.overlay_image_object = None
self.loading_image_pixmap = QtGui.QPixmap.fromImage(
ImageQt(Image.open("Resources/Images/maps_loading.png").resize((1280, 720), Image.BICUBIC)))
self.map_pixmap = self.loading_image_pixmap
self.last_map_pixmap_cache_key = None
self.longitude = 0
self.latitude = 0
self.last_heading = 0
self.imu_data = None
self.new_imu_data = False
self.yaw = None
self.pitch = None
self.roll = None
self.euler_interpolator = interp1d([math.pi, -math.pi], [-180, 180])
self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received)
self.zoom_pan_x_location = 0
self.zoom_pan_y_location = 0
self.zoom_subtraction = 0
self.x_drag = 0
self.y_drag = 0
self.x_drag_start = -1
self.y_drag_start = -1
self.x_drag_end = -1
self.y_drag_end = -1
self.map_selection_name = ""
self.map_selection_latitude = 0
self.map_selection_longitude = 0
self.map_selection_zoom = 0
self.setup_mapping_locations()
def run(self):
self.logger.debug("Starting Map Coordinator Thread")
self.pixmap_ready_signal.emit() # This gets us the loading map
# self.precache_all_maps()
while self.run_thread_flag:
if self.setup_map_flag:
self._map_setup()
self.setup_map_flag = False
else:
if self.new_imu_data:
self.calculate_euler_from_imu()
self.new_imu_data = False
self._get_map_image()
self.msleep(10)
self.logger.debug("Stopping Map Coordinator Thread")
def setup_mapping_locations(self):
locations = [location for location in MappingSettings.MAPPING_LOCATIONS]
self.map_selection_combo_box.addItems(locations)
self.update_settings_from_selection()
self.map_selection_combo_box.setCurrentText(self.map_selection_name)
self.update_zoom_combo_box_items(
MappingSettings.MAPPING_LOCATIONS[MappingSettings.LAST_SELECTION]["valid_zoom_options"],
MappingSettings.LAST_ZOOM_LEVEL)
def update_settings_from_selection(self):
self.map_selection_latitude = MappingSettings.MAPPING_LOCATIONS[MappingSettings.LAST_SELECTION]["latitude"]
self.map_selection_longitude = MappingSettings.MAPPING_LOCATIONS[MappingSettings.LAST_SELECTION]["longitude"]
self.map_selection_zoom = MappingSettings.LAST_ZOOM_LEVEL
self.map_selection_name = MappingSettings.LAST_SELECTION
def precache_all_maps(self):
print "Caching all map options!!!"
for map in MappingSettings.MAPPING_LOCATIONS:
lat = MappingSettings.MAPPING_LOCATIONS[map]["latitude"]
lon = MappingSettings.MAPPING_LOCATIONS[map]["longitude"]
for zoom in MappingSettings.MAPPING_LOCATIONS[map]["valid_zoom_options"]:
print "Caching map: %s at zoom %d" % (map, zoom)
try:
RoverMap.GMapsStitcher(1280,
720,
lat,
lon,
zoom,
'satellite',
None, 20)
except:
print "Could not cache map: %s at zoom %d" % (map, zoom)
print "Finished caching map: %s at zoom %d" % (map, zoom)
print "Map cache complete!"
def _map_setup(self):
self.update_zoom_combo_box_items(
MappingSettings.MAPPING_LOCATIONS[MappingSettings.LAST_SELECTION]["valid_zoom_options"],
MappingSettings.LAST_ZOOM_LEVEL)
self.map_image = None
self.map_pixmap = self.loading_image_pixmap
self.pixmap_ready__slot()
self.google_maps_object = None
self.google_maps_object = RoverMap.GMapsStitcher(1280,
720,
self.map_selection_latitude,
self.map_selection_longitude,
self.map_selection_zoom,
'satellite',
None, 20)
self.overlay_image_object = None
self.overlay_image_object = (
RoverMap.OverlayImage(self.map_selection_latitude, self.map_selection_longitude,
self.google_maps_object.northwest,
self.google_maps_object.southeast,
self.google_maps_object.big_image.size[0],
self.google_maps_object.big_image.size[1],
1280, 720))
def _get_map_image(self):
while self.map_image is None:
self.map_image = self.google_maps_object.display_image
if self.map_image:
self.map_image_copy = self.map_image.copy()
self.update_overlay()
self.map_image = self.map_image_copy.copy()
self.map_image.paste(self.overlay_image_object.display_image,
(0, 0),
self.overlay_image_object.display_image)
# ##### Zoom testing! #####
if self.zoom_subtraction:
self.zoom_subtraction = self.constrain(self.zoom_subtraction, 0, 520)
crop_x_start = ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2) - self.x_drag - self.x_drag_end
crop_y_start = (self.zoom_subtraction / 2) - self.y_drag - self.y_drag_end
crop_x_end = (MAP_WIDGET_WIDTH - (
(self.zoom_subtraction * MAP_WIDGET_RATIO) / 2)) - self.x_drag - self.x_drag_end
crop_y_end = (MAP_WIDGET_HEIGHT - (self.zoom_subtraction / 2)) - self.y_drag - self.y_drag_end
crop_box = (int(crop_x_start), int(crop_y_start), int(crop_x_end), int(crop_y_end))
self.map_image = self.map_image.crop(crop_box)
self.map_image = self.map_image.resize((1280, 720), resample=PIL.Image.BICUBIC)
# ##### Draw coordinates #####
self._draw_coordinate_text(self.latitude, self.longitude)
qim = ImageQt(self.map_image)
self.map_pixmap = QtGui.QPixmap.fromImage(qim)
# if self.map_pixmap.cacheKey() != self.last_map_pixmap_cache_key:
# self.last_map_pixmap_cache_key = self.map_pixmap.cacheKey()
self.pixmap_ready_signal.emit()
def _draw_coordinate_text(self, latitude, longitude):
if latitude == 0 and longitude == 0:
location_text = "LAT: NO FIX\nLON: NO FIX"
else:
location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude)
font = PIL.ImageFont.truetype("UbuntuMono-R", size=20)
new_image = PIL.Image.new('RGBA', (200, 45), "black")
draw = PIL.ImageDraw.Draw(new_image)
draw.multiline_text((5, 0), location_text, font=font)
self.map_image.paste(new_image, (0, 0))
def connect_signals_and_slots(self):
self.pixmap_ready_signal.connect(self.pixmap_ready__slot)
self.change_waypoint_signal.connect(self.update_overlay)
self.mapping_label.mouseReleaseEvent = self.__mouse_released_event
self.mapping_label.wheelEvent = self.__mouse_wheel_event
self.mapping_label.mouseMoveEvent = self.__mouse_move_event
self.map_selection_combo_box.currentTextChanged.connect(self.map_selection_changed__slot)
self.map_zoom_level_combo_box.currentTextChanged.connect(self.zoom_selection_changed__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)
def _get_table_elements(self, UI_element):
temp_list = []
count = UI_element.rowCount()
for row in range(0, count):
name = UI_element.item(row, 0).text()
lat = float(UI_element.item(row, 1).text())
lng = float(UI_element.item(row, 2).text())
color = UI_element.item(row, 3).background().color()
temp_tuple = (name, lat, lng, color)
temp_list.append(temp_tuple)
return temp_list
def update_overlay(self):
if not numpy.isnan(self.latitude) and not numpy.isnan(self.longitude):
latitude = float(self.latitude)
longitude = float(self.longitude)
navigation_list = self._get_table_elements(self.navigation_label)
landmark_list = self._get_table_elements(self.landmark_label)
self.overlay_image = self.overlay_image_object.update_new_location(
latitude,
longitude,
self.last_heading,
navigation_list,
landmark_list)
def gps_position_updated_callback(self, data):
self.latitude = data.latitude
self.longitude = data.longitude
def on_imu_data_received(self, data):
self.imu_data = data
self.new_imu_data = True
def calculate_euler_from_imu(self):
quat = (
self.imu_data.orientation.x,
self.imu_data.orientation.y,
self.imu_data.orientation.z,
self.imu_data.orientation.w,
)
self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat)
self.last_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360
def __mouse_released_event(self, _):
self.x_drag_start = -1
self.y_drag_start = -1
self.x_drag_end += self.x_drag
self.y_drag_end += self.y_drag
self.x_drag = 0
self.x_drag = 0
def __mouse_wheel_event(self, event):
self.zoom_subtraction += event.angleDelta().y() / 12
def __mouse_move_event(self, event):
if self.x_drag_start != -1 and self.x_drag_start != -1:
buttons = event.buttons()
if buttons == QtCore.Qt.LeftButton:
x_pos = event.pos().x()
y_pos = event.pos().y()
dx = x_pos - self.x_drag_start
dy = y_pos - self.y_drag_start
self.x_drag = dx
self.y_drag = dy
else:
self.x_drag_start = event.pos().x()
self.y_drag_start = event.pos().y()
def update_zoom_combo_box_items(self, zooms, selection):
self.map_zoom_level_combo_box.clear()
self.map_zoom_level_combo_box.addItems([str(item) for item in zooms])
self.map_zoom_level_combo_box.setCurrentText(str(selection))
def zoom_selection_changed__slot(self, zoom):
if zoom:
MappingSettings.LAST_ZOOM_LEVEL = int(zoom)
self.map_selection_zoom = int(zoom)
self.setup_map_flag = True
def map_selection_changed__slot(self, selection):
MappingSettings.LAST_SELECTION = selection
MappingSettings.LAST_ZOOM_LEVEL = MappingSettings.MAPPING_LOCATIONS[selection]["default_zoom"]
self.update_settings_from_selection()
self.setup_map_flag = True
self.map_selection_name = selection
@staticmethod
def constrain(val, min_val, max_val):
return min(max_val, max(min_val, val))

46
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapHelper.py Normal file
View File

@@ -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, 0))
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
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MapSystems/__init__.py Normal file
View File

53
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MapSystems/signing.py Normal file
View File

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

201
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py Normal file
View File

@@ -0,0 +1,201 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets, QtGui
import logging
from time import time
import paramiko
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 5
IP = "192.168.1.10"
USER = "nvidia"
PASS = "nvidia"
#####################################
# BashConsole Class Definition
#####################################
class BashConsole(QtCore.QThread):
text_update_ready__signal = QtCore.pyqtSignal(str)
def __init__(self, shared_objects):
super(BashConsole, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.console_text_edit = self.left_screen.console_line_edit # type: QtWidgets.QTextEdit
self.ssh_console_command_line_edit = self.left_screen.ssh_console_command_line_edit # type:QtWidgets.QLineEdit
self.ssh_scan_for_hosts_button = self.left_screen.ssh_scan_for_hosts_button # type: QtWidgets.QPushButton
self.ssh_host_line_edit = self.left_screen.ssh_host_line_edit # type: QtWidgets.QLineEdit
self.ssh_list_wifi_button = self.left_screen.ssh_list_wifi_button # type: QtWidgets.QPushButton
self.ssh_equipment_login_button = self.left_screen.ssh_equipment_login_button # type: QtWidgets.QPushButton
self.ssh_equipment_logout_button = self.left_screen.ssh_equipment_logout_button # type: QtWidgets.QPushButton
self.ssh_equipment_status_button = self.left_screen.ssh_equipment_status_button # type: QtWidgets.QPushButton
self.ssh_equipment_start_button = self.left_screen.ssh_equipment_start_button # type: QtWidgets.QPushButton
self.ssh_equipment_stop_button = self.left_screen.ssh_equipment_stop_button # type: QtWidgets.QPushButton
self.ssh_ssid_line_edit = self.left_screen.ssh_ssid_line_edit # type:QtWidgets.QLineEdit
self.ssh_connect_ssid_push_button = self.left_screen.ssh_ssid_push_button # type: QtWidgets.QPushButton
self.ssh_disconnect_wifi_button = self.left_screen.ssh_disconnect_wifi_button # type: QtWidgets.QPushButton
# ########## 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.bash_process = None
self.new_widget = None
self.window = None
self.wait_time = 1.0 / THREAD_HERTZ
self.ssh_client = None
self.set_text_contents = ""
self.new_command_text = ""
self.new_command = False
def run(self):
while not self.ssh_client:
try:
self.ssh_client = paramiko.SSHClient()
self.ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
self.ssh_client.connect(IP, username=USER, password=PASS, compress=True)
except:
print "No connection"
if not self.run_thread_flag:
return
self.ssh_client = None
self.msleep(1000)
while self.run_thread_flag:
start_time = time()
if self.new_command:
_, ssh_stdout, ssh_stderr = self.ssh_client.exec_command(self.new_command_text)
stdout_read = ssh_stdout.read()
stderr_read = ssh_stderr.read()
output = ""
output += "\n%s@%s:$" % (USER, IP)
output += self.new_command_text + "\n"
output += stdout_read.decode("utf-8") if stdout_read else ""
output += stderr_read.decode("utf-8") if stderr_read else ""
self.set_text_contents += output
self.text_update_ready__signal.emit(self.set_text_contents)
self.new_command = False
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
del self.bash_process
def on_text_readout_updated__slot(self):
self.console_text_edit.moveCursor(QtGui.QTextCursor.End)
def on_text_editing_finished__slot(self):
self.new_command_text = self.ssh_console_command_line_edit.text()
self.new_command = True
def on_list_wifi_button_pressed__slot(self):
self.new_command_text = "nmcli dev wifi list"
self.new_command = True
def on_login_button_pressed__slot(self):
current_ip = self.ssh_host_line_edit.text()
self.new_command_text = "python equipment_servicing_interface.py '%s' 'LOGIN MTECH GITRDONE' HELP" % current_ip
print self.new_command_text
self.new_command = True
def on_logout_button_pressed__slot(self):
current_ip = self.ssh_host_line_edit.text()
self.new_command_text = "python equipment_servicing_interface.py '%s' LOGOUT" % current_ip
self.new_command = True
def on_status_button_pressed__slot(self):
current_ip = self.ssh_host_line_edit.text()
self.new_command_text = "python equipment_servicing_interface.py '%s' STATUS" % current_ip
self.new_command = True
def on_start_button_pressed__slot(self):
current_ip = self.ssh_host_line_edit.text()
self.new_command_text = "python equipment_servicing_interface.py '%s' START" % current_ip
self.new_command = True
def on_stop_button_pressed__slot(self):
current_ip = self.ssh_host_line_edit.text()
self.new_command_text = "python equipment_servicing_interface.py '%s' STOP" % current_ip
self.new_command = True
def on_ssh_scan_for_hosts_pressed__slot(self):
current_ip = self.ssh_host_line_edit.text()
find_dot = current_ip.rfind(".")
if find_dot > 0:
current_ip = current_ip[:find_dot + 1] + "0"
self.new_command_text = "nmap -sP %s/24 -oG - | awk '/Up$/{print $2}'" % current_ip
self.new_command = True
else:
self.set_text_contents += "IP address for range search not valid. Try again."
self.text_update_ready__signal.emit(self.set_text_contents)
def on_connect_ssid_button_pressed__slot(self):
ssid_text = self.ssh_ssid_line_edit.text()
self.new_command_text = "sudo nmcli dev wifi connect %s" % ssid_text
self.new_command = True
def on_disconnect_ssid_button_pressed__slot(self):
ssid_text = self.ssh_ssid_line_edit.text()
self.new_command_text = "sudo nmcli con down id %s ; sudo nmcli connection delete %s" % (ssid_text, ssid_text)
self.new_command = True
def connect_signals_and_slots(self):
self.text_update_ready__signal.connect(self.console_text_edit.setText)
self.ssh_console_command_line_edit.editingFinished.connect(self.on_text_editing_finished__slot)
self.console_text_edit.textChanged.connect(self.on_text_readout_updated__slot)
self.ssh_scan_for_hosts_button.clicked.connect(self.on_ssh_scan_for_hosts_pressed__slot)
self.ssh_equipment_login_button.clicked.connect(self.on_login_button_pressed__slot)
self.ssh_equipment_logout_button.clicked.connect(self.on_logout_button_pressed__slot)
self.ssh_equipment_status_button.clicked.connect(self.on_status_button_pressed__slot)
self.ssh_equipment_start_button.clicked.connect(self.on_start_button_pressed__slot)
self.ssh_equipment_stop_button.clicked.connect(self.on_stop_button_pressed__slot)
self.ssh_list_wifi_button.clicked.connect(self.on_list_wifi_button_pressed__slot)
self.ssh_connect_ssid_push_button.clicked.connect(self.on_connect_ssid_button_pressed__slot)
self.ssh_disconnect_wifi_button.clicked.connect(self.on_disconnect_ssid_button_pressed__slot)
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

257
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py Normal file
View File

@@ -0,0 +1,257 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
import rospy
from rover_control.msg import MiningStatusMessage, MiningControlMessage, CameraControlMessage
from rover_science.msg import SoilSensorStatusMessage
from std_msgs.msg import Float64
#####################################
# Global Variables
#####################################
MINING_STATUS_TOPIC = "/rover_control/mining/status"
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
SCALE_MEASUREMENT_TOPIC = "/rover_control/scale/measurement"
SOIL_PROBE_TOPIC = "/rover_science/soil_probe/data"
CAMERA_CONTROL_TOPIC = "/rover_control/camera/control"
TRAVEL_POSITION_LIFT = 110
TRAVEL_POSITION_TILT = 1023
MEASURE_POSITION_LIFT = 350
MEASURE_POSITION_TILT = 1023
SCOOP_POSITION_LIFT = 228
SCOOP_POSITION_TILT = 215
PANORAMA_POSITION_LIFT = 535
PANORAMA_POSITION_TILT = 995
SAMPLE_POSITION_LIFT = 220
SAMPLE_POSITION_TILT = 240
PROBE_POSITION_LIFT = 950
PROBE_POSITION_TILT = 440
#####################################
# UbiquitiRadioSettings Class Definition
#####################################
class Mining(QtCore.QObject):
lift_position_update_ready__signal = QtCore.pyqtSignal(int)
tilt_position_update_ready__signal = QtCore.pyqtSignal(int)
weight_measurement_update_ready__signal = QtCore.pyqtSignal(int)
temp_update_ready__signal = QtCore.pyqtSignal(float)
moisture_update_ready__signal = QtCore.pyqtSignal(float)
loss_tangent_update_ready__signal = QtCore.pyqtSignal(float)
electrical_conductivity_update_ready__signal = QtCore.pyqtSignal(float)
real_dielectric_update_ready__signal = QtCore.pyqtSignal(float)
imaginary_dielectric_update_ready__signal = QtCore.pyqtSignal(float)
def __init__(self, shared_objects):
super(Mining, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.mining_qlcdnumber = self.left_screen.mining_qlcdnumber # type:QtWidgets.QLCDNumber
self.mining_zero_button = self.left_screen.mining_zero_button # type:QtWidgets.QPushButton
self.lift_position_progress_bar = self.left_screen.lift_position_progress_bar # type:QtWidgets.QProgressBar
self.tilt_position_progress_bar = self.left_screen.tilt_position_progress_bar # type:QtWidgets.QProgressBar
self.mining_measure_move_button = self.left_screen.mining_measure_move_button # type:QtWidgets.QPushButton
self.mining_transport_move_button = self.left_screen.mining_transport_move_button # type:QtWidgets.QPushButton
self.mining_scoop_move_button = self.left_screen.mining_scoop_move_button # type:QtWidgets.QPushButton
self.mining_panorama_button = self.left_screen.mining_panorama_button # type:QtWidgets.QPushButton
self.mining_sample_button = self.left_screen.mining_sample_button # type:QtWidgets.QPushButton
self.mining_probe_button = self.left_screen.mining_probe_button # type:QtWidgets.QPushButton
self.science_temp_lcd_number = self.left_screen.science_temp_lcd_number # type:QtWidgets.QLCDNumber
self.science_moisture_lcd_number = self.left_screen.science_moisture_lcd_number # type:QtWidgets.QLCDNumber
self.science_loss_tangent_lcd_number = self.left_screen.science_loss_tangent_lcd_number # type:QtWidgets.QLCDNumber
self.science_electrical_conductivity_lcd_number = self.left_screen.science_electrical_conductivity_lcd_number # type:QtWidgets.QLCDNumber
self.science_real_dielectric_lcd_number = self.left_screen.science_real_dielectric_lcd_number # type:QtWidgets.QLCDNumber
self.science_imaginary_dielectric_lcd_number = self.left_screen.science_imaginary_dielectric_lcd_number # type:QtWidgets.QLCDNumber
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_network_output_button = self.left_screen.cam_network_output_button # type:QtWidgets.QPushButton
self.cam_zoom_in_button = self.left_screen.cam_zoom_in_button # type:QtWidgets.QPushButton
self.cam_zoom_out_button = self.left_screen.cam_zoom_out_button # type:QtWidgets.QPushButton
self.cam_full_zoom_in_button = self.left_screen.cam_full_zoom_in_button # type:QtWidgets.QPushButton
self.cam_full_zoom_out_button = self.left_screen.cam_full_zoom_out_button # type:QtWidgets.QPushButton
self.cam_shoot_button = self.left_screen.cam_shoot_button # type:QtWidgets.QPushButton
# ########## 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.mining_status_subscriber = rospy.Subscriber(MINING_STATUS_TOPIC, MiningStatusMessage,
self.mining_status_message_received__callback)
self.soil_probe_subscriber = rospy.Subscriber(SOIL_PROBE_TOPIC, SoilSensorStatusMessage, self.on_soil_probe_message_received__callback)
self.scale_measurement_subscriber = rospy.Subscriber(SCALE_MEASUREMENT_TOPIC, Float64, self.on_scale_measurement_received__callback)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.camera_control_publisher = rospy.Publisher(CAMERA_CONTROL_TOPIC, CameraControlMessage, queue_size=1)
self.current_scale_measurement = 0
self.scale_zero_offset = 0
self.connect_signals_and_slots()
def connect_signals_and_slots(self):
self.mining_zero_button.clicked.connect(self.on_mining_zero_clicked__slot)
self.mining_measure_move_button.clicked.connect(self.on_mining_move_measure_clicked__slot)
self.mining_transport_move_button.clicked.connect(self.on_mining_move_transport_clicked__slot)
self.mining_scoop_move_button.clicked.connect(self.on_mining_move_scoop_clicked__slot)
self.mining_panorama_button.clicked.connect(self.on_mining_move_panorama_clicked__slot)
self.mining_sample_button.clicked.connect(self.on_mining_move_sample_clicked__slot)
self.mining_probe_button.clicked.connect(self.on_mining_move_probe_clicked__slot)
self.tilt_position_update_ready__signal.connect(self.tilt_position_progress_bar.setValue)
self.lift_position_update_ready__signal.connect(self.lift_position_progress_bar.setValue)
self.weight_measurement_update_ready__signal.connect(self.mining_qlcdnumber.display)
self.temp_update_ready__signal.connect(self.science_temp_lcd_number.display)
self.moisture_update_ready__signal.connect(self.science_moisture_lcd_number.display)
self.loss_tangent_update_ready__signal.connect(self.science_loss_tangent_lcd_number.display)
self.electrical_conductivity_update_ready__signal.connect(self.science_electrical_conductivity_lcd_number.display)
self.real_dielectric_update_ready__signal.connect(self.science_real_dielectric_lcd_number.display)
self.imaginary_dielectric_update_ready__signal.connect(self.science_imaginary_dielectric_lcd_number.display)
self.cam_lcd_output_button.clicked.connect(self.on_cam_lcd_button_clicked__slot)
self.cam_network_output_button.clicked.connect(self.on_cam_network_button_clicked__slot)
self.cam_zoom_in_button.clicked.connect(self.on_cam_zoom_in_button_clicked__slot)
self.cam_zoom_out_button.clicked.connect(self.on_cam_zoom_out_button_clicked__slot)
self.cam_full_zoom_in_button.clicked.connect(self.on_cam_full_zoom_in_button_clicked__slot)
self.cam_full_zoom_out_button.clicked.connect(self.on_cam_full_zoom_out_button_clicked__slot)
self.cam_shoot_button.clicked.connect(self.on_cam_shoot_button_clicked__slot)
def on_mining_zero_clicked__slot(self):
self.scale_zero_offset = -self.current_scale_measurement
def on_mining_move_transport_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = TRAVEL_POSITION_TILT
message.lift_set_absolute = TRAVEL_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_measure_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = MEASURE_POSITION_TILT
message.lift_set_absolute = MEASURE_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_scoop_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = SCOOP_POSITION_TILT
message.lift_set_absolute = SCOOP_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_panorama_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = PANORAMA_POSITION_TILT
message.lift_set_absolute = PANORAMA_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_sample_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = SAMPLE_POSITION_TILT
message.lift_set_absolute = SAMPLE_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_probe_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = PROBE_POSITION_TILT
message.lift_set_absolute = PROBE_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_cam_lcd_button_clicked__slot(self):
message = CameraControlMessage()
message.camera_mode = 1
self.camera_control_publisher.publish(message)
def on_cam_network_button_clicked__slot(self):
message = CameraControlMessage()
message.camera_mode = 2
self.camera_control_publisher.publish(message)
def on_cam_zoom_in_button_clicked__slot(self):
message = CameraControlMessage()
message.zoom_in = 1
self.camera_control_publisher.publish(message)
def on_cam_zoom_out_button_clicked__slot(self):
message = CameraControlMessage()
message.zoom_out = 1
self.camera_control_publisher.publish(message)
def on_cam_full_zoom_in_button_clicked__slot(self):
message = CameraControlMessage()
message.full_zoom_in = 1
self.camera_control_publisher.publish(message)
def on_cam_full_zoom_out_button_clicked__slot(self):
message = CameraControlMessage()
message.full_zoom_out = 1
self.camera_control_publisher.publish(message)
def on_cam_shoot_button_clicked__slot(self):
message = CameraControlMessage()
message.shoot = 1
self.camera_control_publisher.publish(message)
def mining_status_message_received__callback(self, status):
status = status # type:MiningStatusMessage
self.tilt_position_update_ready__signal.emit(status.tilt_position)
self.lift_position_update_ready__signal.emit(status.lift_position)
def on_soil_probe_message_received__callback(self, data):
self.temp_update_ready__signal.emit(data.temp_c)
self.moisture_update_ready__signal.emit(data.moisture)
self.loss_tangent_update_ready__signal.emit(data.loss_tangent)
self.electrical_conductivity_update_ready__signal.emit(data.soil_electrical_conductivity)
self.real_dielectric_update_ready__signal.emit(data.real_dielectric_permittivity)
self.imaginary_dielectric_update_ready__signal.emit(data.imaginary_dielectric_permittivity)
def on_scale_measurement_received__callback(self, data):
grams = data.data * 1000
self.current_scale_measurement = grams
self.weight_measurement_update_ready__signal.emit(grams + self.scale_zero_offset)

312
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py Normal file
View File

@@ -0,0 +1,312 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
import rospy
from time import time
from rover_arm.msg import ArmControlMessage, ArmStatusMessage
from rover_control.msg import GripperControlMessage
#####################################
# Global Variables
#####################################
ARM_RELATIVE_CONTROL_TOPIC = "/rover_arm/control/relative"
ARM_ABSOLUTE_CONTROL_TOPIC = "/rover_arm/control/absolute"
ARM_STATUS_TOPIC = "/rover_arm/status"
GRIPPER_CONTROL_TOPIC = "/rover_control/gripper/control"
THREAD_HERTZ = 5
POSITIONAL_TOLERANCE = 0.02
# Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll]
ARM_STOW_PROCEDURE = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, -0.035, -0.28, -0.25, 0.25, 0.0],
[0.0, -0.035, -0.5, -0.25, 0.25, 0.0],
[0.0, -0.25, -0.5, -0.25, 0.25, -0.25]
]
ARM_UNSTOW_PROCEDURE = [
[0.0, -0.25, -0.5, -0.25, 0.25, -0.25],
[0.0, -0.035, -0.5, -0.25, 0.25, 0.0],
[0.0, -0.035, -0.28, -0.25, 0.25, 0.0],
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0]
]
APPROACH_O2 = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, 0.0485472094896, -0.273685193022, -0.00102834607876, -0.17200773156, 0], # Correct positioning out to left side of rover
[-0.436250296246, 0.0485472094896, -0.273685193022, -0.00102834607876, -0.17200773156, 0] # Directly positioned, ready to grip
]
APPROACH_BEACON = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, 0.0361371382336, -0.325145836608, -0.00731261537597, -0.129662333807, 0.0569339269566], # Correct positioning out to right side of rover
[0.458505849045, 0.0361371382336, -0.325145633259, -0.00731200471916, -0.131140162948, 0.0920117742056]
]
#####################################
# UbiquitiRadioSettings Class Definition
#####################################
class MiscArm(QtCore.QThread):
def __init__(self, shared_objects):
super(MiscArm, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.arm_control_upright_zeroed_button = self.left_screen.arm_control_upright_zeroed_button # type:QtWidgets.QPushButton
self.arm_controls_stow_arm_button = self.left_screen.arm_controls_stow_arm_button # type:QtWidgets.QPushButton
self.arm_controls_unstow_arm_button = self.left_screen.arm_controls_unstow_arm_button # type:QtWidgets.QPushButton
# ##### FIXME #####
# Remove these once the arm is fixed
self.arm_controls_stow_arm_button.setEnabled(False)
self.arm_controls_unstow_arm_button.setEnabled(False)
# #################
self.arm_controls_calibration_button = self.left_screen.arm_controls_calibration_button # type:QtWidgets.QPushButton
self.arm_controls_clear_faults_button = self.left_screen.arm_controls_clear_faults_button # type:QtWidgets.QPushButton
self.arm_controls_reset_motor_drivers_button = self.left_screen.arm_controls_reset_motor_drivers_button # type:QtWidgets.QPushButton
self.arm_controls_approach_o2_button = self.left_screen.arm_controls_approach_o2_button # type:QtWidgets.QPushButton
self.arm_controls_depart_o2_button = self.left_screen.arm_controls_depart_o2_button # type:QtWidgets.QPushButton
self.arm_controls_approach_beacon_button = self.left_screen.arm_controls_approach_beacon_button # type:QtWidgets.QPushButton
self.arm_controls_depart_beacon_button = self.left_screen.arm_controls_depart_beacon_button # type:QtWidgets.QPushButton
self.gripper_home_button = self.left_screen.gripper_home_button # type:QtWidgets.QPushButton
self.gripper_toggle_light_button = self.left_screen.gripper_toggle_light_button # type:QtWidgets.QPushButton
# ########## 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.wait_time = 1.0 / THREAD_HERTZ
self.arm_status_subscriber = rospy.Subscriber(ARM_STATUS_TOPIC, ArmStatusMessage,
self.new_arm_status_message_received__callback)
self.arm_relative_control_publisher = rospy.Publisher(ARM_RELATIVE_CONTROL_TOPIC, ArmControlMessage,
queue_size=1)
self.arm_absolute_control_publisher = rospy.Publisher(ARM_ABSOLUTE_CONTROL_TOPIC, ArmControlMessage,
queue_size=1)
self.gripper_control_publisher = rospy.Publisher(GRIPPER_CONTROL_TOPIC, GripperControlMessage, queue_size=1)
self.base_position = 0
self.shoulder_position = 0
self.elbow_position = 0
self.roll_position = 0
self.wrist_pitch_position = 0
self.wrist_roll_position = 0
self.should_stow_arm = False
self.should_unstow_arm = False
self.should_approach_o2 = False
self.should_depart_o2 = False
self.should_approach_beacon = False
self.should_depart_beacon = False
def run(self):
self.logger.debug("Starting MiscArm Thread")
while self.run_thread_flag:
start_time = time()
if self.should_stow_arm:
self.stow_rover_arm()
self.should_stow_arm = False
elif self.should_unstow_arm:
self.unstow_rover_arm()
self.should_unstow_arm = False
elif self.should_approach_o2:
self.approach_o2()
self.should_approach_o2 = False
elif self.should_depart_o2:
self.depart_o2()
self.should_depart_o2 = False
elif self.should_approach_beacon:
self.approach_beacon()
self.should_approach_beacon = False
elif self.should_depart_beacon:
self.depart_beacon()
self.should_depart_beacon = False
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
self.logger.debug("Stopping MiscArm Thread")
def stow_rover_arm(self):
for movement in ARM_STOW_PROCEDURE:
self.process_absolute_move_command(movement)
def unstow_rover_arm(self):
for movement in ARM_UNSTOW_PROCEDURE:
self.process_absolute_move_command(movement)
def approach_o2(self):
for movement in APPROACH_O2:
self.process_absolute_move_command(movement)
def depart_o2(self):
for movement in reversed(APPROACH_O2):
self.process_absolute_move_command(movement)
def approach_beacon(self):
for movement in APPROACH_BEACON:
self.process_absolute_move_command(movement)
def depart_beacon(self):
for movement in reversed(APPROACH_BEACON):
self.process_absolute_move_command(movement)
def process_absolute_move_command(self, movement):
message = ArmControlMessage()
message.base = movement[0]
message.shoulder = movement[1]
message.elbow = movement[2]
message.roll = movement[3]
message.wrist_pitch = movement[4]
message.wrist_roll = movement[5]
self.arm_absolute_control_publisher.publish(message)
self.wait_for_targets_reached(movement)
self.msleep(250)
def wait_for_targets_reached(self, movement):
base_set = movement[0]
shoulder_set = movement[1]
elbow_set = movement[2]
roll_set = movement[3]
wrist_pitch_set = movement[4]
wrist_roll_set = movement[5] - (wrist_pitch_set / 2.0)
while abs(self.base_position - base_set) > POSITIONAL_TOLERANCE:
# self.logger.debug("Waiting for base| %f\t%f" % (self.base_position, base_set))
self.msleep(10)
while abs(self.shoulder_position - shoulder_set) > POSITIONAL_TOLERANCE:
# self.logger.debug("Waiting for shoulder| %f\t%f" % (self.shoulder_position, shoulder_set))
self.msleep(10)
while abs(self.elbow_position - elbow_set) > POSITIONAL_TOLERANCE:
# self.logger.debug("Waiting for elbow| %f\t%f" % (self.elbow_position, elbow_set))
self.msleep(10)
while abs(self.roll_position - roll_set) > POSITIONAL_TOLERANCE:
# self.logger.debug("Waiting for roll| %f\t%f" % (self.roll_position, roll_set))
self.msleep(10)
while abs(self.wrist_pitch_position - wrist_pitch_set) > POSITIONAL_TOLERANCE:
# self.logger.debug("Waiting for wrist_pitch| %f\t%f" % (self.wrist_pitch_position, wrist_pitch_set))
self.msleep(10)
while abs(self.wrist_roll_position - wrist_roll_set) > POSITIONAL_TOLERANCE:
# self.logger.debug("Waiting for wrist_roll| %f\t%f" % (self.wrist_roll_position, wrist_roll_set))
self.msleep(10)
def connect_signals_and_slots(self):
self.arm_controls_stow_arm_button.clicked.connect(self.on_stow_arm_button_pressed__slot)
self.arm_controls_unstow_arm_button.clicked.connect(self.on_unstow_arm_button_pressed__slot)
self.arm_control_upright_zeroed_button.clicked.connect(self.on_upright_zeroed_button_pressed__slot)
self.arm_controls_calibration_button.clicked.connect(self.on_set_calibration_button_pressed__slot)
self.arm_controls_clear_faults_button.clicked.connect(self.on_clear_faults_button_pressed__slot)
self.arm_controls_reset_motor_drivers_button.clicked.connect(self.on_reset_drivers_button_pressed__slot)
self.arm_controls_approach_o2_button.clicked.connect(self.on_approach_o2_button_pressed__slot)
self.arm_controls_approach_beacon_button.clicked.connect(self.on_approach_beacon_button_pressed__slot)
self.gripper_home_button.clicked.connect(self.on_gripper_home_pressed)
self.gripper_toggle_light_button.clicked.connect(self.on_gripper_toggle_light_pressed)
def on_upright_zeroed_button_pressed__slot(self):
self.process_absolute_move_command([0 for _ in range(6)])
def on_set_calibration_button_pressed__slot(self):
message = ArmControlMessage()
message.calibrate = True
self.arm_relative_control_publisher.publish(message)
def on_clear_faults_button_pressed__slot(self):
message = ArmControlMessage()
message.clear_faults = True
self.arm_relative_control_publisher.publish(message)
def on_reset_drivers_button_pressed__slot(self):
message = ArmControlMessage()
message.reset_controllers = True
self.arm_relative_control_publisher.publish(message)
def on_stow_arm_button_pressed__slot(self):
self.should_stow_arm = True
def on_unstow_arm_button_pressed__slot(self):
self.should_unstow_arm = True
def on_approach_o2_button_pressed__slot(self):
self.should_approach_o2 = True
def on_depart_o2_button_pressed__slot(self):
self.should_depart_o2 = True
def on_approach_beacon_button_pressed__slot(self):
self.should_approach_beacon = True
def on_depart_beacon_button_pressed__slot(self):
self.should_depart_beacon = True
def on_gripper_home_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 2
message.gripper_position_absolute = -1
message.should_home = True
self.gripper_control_publisher.publish(message)
def on_gripper_toggle_light_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 2
message.toggle_light = True
message.gripper_position_absolute = -1
self.gripper_control_publisher.publish(message)
def new_arm_status_message_received__callback(self, data):
self.base_position = data.base
self.shoulder_position = data.shoulder
self.elbow_position = data.elbow
self.roll_position = data.roll
self.wrist_pitch_position = data.wrist_pitch
self.wrist_roll_position = data.wrist_roll
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

166
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py Normal file
View File

@@ -0,0 +1,166 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
import rospy
from time import time
import scipy.fftpack
import numpy
from std_msgs.msg import Float64MultiArray
#####################################
# Global Variables
#####################################
RDF_DATA_TOPIC = "/rover_science/rdf/data"
THREAD_HERTZ = 5
COLOR_GREEN = "background-color:darkgreen;"
COLOR_RED = "background-color:darkred;"
ALLOWED_RDF_VARIANCE = 0.15
#####################################
# UbiquitiRadioSettings Class Definition
#####################################
class RDF(QtCore.QThread):
rssi_lcd_number_update_ready__signal = QtCore.pyqtSignal(int)
beacon_lcd_number_update_ready__signal = QtCore.pyqtSignal(float)
beacon_valid_text_change_ready__signal = QtCore.pyqtSignal(str)
beacon_valid_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
def __init__(self, shared_objects):
super(RDF, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.rssi_lcdnumber = self.left_screen.rssi_lcdnumber # type:QtWidgets.QLCDNumber
self.beacon_frequency_lcd_number = self.left_screen.beacon_frequency_lcd_number # type:QtWidgets.QLCDNumber
self.beacon_frequency_valid_label = self.left_screen.beacon_frequency_valid_label # type:QtWidgets.QLCDNumber
# ########## 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.wait_time = 1.0 / THREAD_HERTZ
self.rdf_subscriber = rospy.Subscriber(RDF_DATA_TOPIC, Float64MultiArray, self.new_rdf_message_received__callback)
self.moving_average_raw_data = []
self.raw_data = numpy.array([])
self.raw_data_timestamps = numpy.array([])
self.data_window_size = 200
self.previous_frequencies = []
self.num_previous_frequencies = 3
def run(self):
self.logger.debug("Starting RDF Thread")
while self.run_thread_flag:
start_time = time()
temp = list(self.moving_average_raw_data)
if temp:
average = sum(temp) / len(temp)
self.rssi_lcd_number_update_ready__signal.emit(average)
if self.raw_data.size >= self.data_window_size and self.raw_data_timestamps.size >= self.data_window_size:
try:
time_step_array = numpy.array([])
for n in range(0, self.data_window_size - 1):
time_step_array = numpy.append(time_step_array, self.raw_data_timestamps[n + 1] - self.raw_data_timestamps[n])
T = numpy.average(time_step_array)
yf = scipy.fftpack.fft(self.raw_data)
xf = numpy.linspace(0.0, 1.0 / (2.0 * T), self.data_window_size / 2)
valid_range = []
for n in range(0, len(xf)):
if (xf[n] > 0.5) and (xf[n] <= 5.0):
valid_range.append(n)
yf = numpy.take(yf, valid_range)
xf = numpy.take(xf, valid_range)
max_index = numpy.argmax(numpy.abs(yf))
freq = xf[max_index]
if len(self.previous_frequencies) == self.num_previous_frequencies:
del self.previous_frequencies[0]
self.previous_frequencies.append(freq)
if len(self.previous_frequencies) == self.num_previous_frequencies:
variance_too_large = False
if abs(self.previous_frequencies[0] - self.previous_frequencies[1]) > ALLOWED_RDF_VARIANCE:
variance_too_large = True
if abs(self.previous_frequencies[0] - self.previous_frequencies[2]) > ALLOWED_RDF_VARIANCE:
variance_too_large = True
if abs(self.previous_frequencies[1] - self.previous_frequencies[2]) > ALLOWED_RDF_VARIANCE:
variance_too_large = True
self.beacon_valid_stylesheet_change_ready__signal.emit(COLOR_GREEN if not variance_too_large else COLOR_RED)
self.beacon_valid_text_change_ready__signal.emit("Yes" if not variance_too_large else "No")
self.beacon_lcd_number_update_ready__signal.emit(freq)
except Exception, e:
print e
self.raw_data = numpy.array([])
self.raw_data_timestamps = numpy.array([])
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
self.logger.debug("Stopping RDF Thread")
def new_rdf_message_received__callback(self, data):
if len(self.moving_average_raw_data) >= self.data_window_size:
del self.moving_average_raw_data[0]
self.moving_average_raw_data.append(data.data[0])
if self.raw_data.size != self.data_window_size and self.raw_data_timestamps.size != self.data_window_size:
self.raw_data = numpy.append(self.raw_data, data.data[0])
self.raw_data_timestamps = numpy.append(self.raw_data_timestamps, data.data[1])
def connect_signals_and_slots(self):
self.rssi_lcd_number_update_ready__signal.connect(self.rssi_lcdnumber.display)
self.beacon_lcd_number_update_ready__signal.connect(self.beacon_frequency_lcd_number.display)
self.beacon_valid_text_change_ready__signal.connect(self.beacon_frequency_valid_label.setText)
self.beacon_valid_stylesheet_change_ready__signal.connect(self.beacon_frequency_valid_label.setStyleSheet)
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
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/MiscSystems/__init__.py Normal file
View File

182
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py Normal file
View File

@@ -0,0 +1,182 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets, QtGui
import logging
from time import time
import PIL.Image
from PIL.ImageQt import ImageQt
from random import random
import rospy
from tf import transformations
from scipy.interpolate import interp1d
import math
from sensor_msgs.msg import Imu
from Resources.Settings import MappingSettings as MappingSettings
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 20
ROTATION_SPEED_MODIFIER = 2.5
IMU_DATA_TOPIC = "/rover_odometry/imu/data"
#####################################
# Controller Class Definition
#####################################
class SpeedAndHeadingIndication(QtCore.QThread):
show_compass_image__signal = QtCore.pyqtSignal()
heading_text_update_ready__signal = QtCore.pyqtSignal(str)
new_speed_update_ready__signal = QtCore.pyqtSignal(str)
pitch_update_ready__signal = QtCore.pyqtSignal(float)
roll_update_ready__signal = QtCore.pyqtSignal(float)
def __init__(self, shared_objects):
super(SpeedAndHeadingIndication, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.right_screen = self.shared_objects["screens"]["right_screen"]
self.heading_compass_label = self.right_screen.heading_compass_label # type: QtWidgets.QLabel
self.heading_text_label = self.right_screen.current_heading_label # type: QtWidgets.QLabel
self.next_goal_label = self.right_screen.next_goal_label # type: QtWidgets.QLabel
self.current_speed_label = self.right_screen.current_speed_label # type: QtWidgets.QLabel
self.imu_pitch_lcd_number = self.right_screen.imu_pitch_lcd_number # type: QtWidgets.QLCDNumber
self.imu_roll_lcd_number = self.right_screen.imu_roll_lcd_number # type: QtWidgets.QLCDNumber
# ########## 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.wait_time = 1.0 / THREAD_HERTZ
self.main_compass_image = PIL.Image.open("Resources/Images/compass.png").resize((300, 300), PIL.Image.ANTIALIAS)
self.compass_pixmap = None
self.current_heading = 0
self.current_heading_changed = True
self.shown_heading = (self.current_heading + (1.5 * ROTATION_SPEED_MODIFIER)) % 360
self.current_heading_shown_rotation_angle = 0
self.last_current_heading_shown = -1000
self.rotation_direction = 1
self.imu_data = None
self.new_imu_data = False
self.yaw = None
self.pitch = None
self.roll = None
self.euler_interpolator = interp1d([math.pi, -math.pi], [-180, 180])
self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received)
def run(self):
self.on_heading_changed__slot(self.current_heading)
while self.run_thread_flag:
start_time = time()
if self.new_imu_data:
self.calculate_euler_from_imu()
self.new_imu_data = False
if self.current_heading_changed:
self.update_heading_movement()
self.current_heading_changed = False
self.rotate_compass_if_needed()
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
def calculate_euler_from_imu(self):
quat = (
self.imu_data.orientation.x,
self.imu_data.orientation.y,
self.imu_data.orientation.z,
self.imu_data.orientation.w,
)
self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat)
self.current_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360
self.pitch_update_ready__signal.emit(self.pitch)
self.roll_update_ready__signal.emit(self.roll)
def rotate_compass_if_needed(self):
self.current_heading_shown_rotation_angle = int(self.current_heading)
if self.current_heading_shown_rotation_angle != self.last_current_heading_shown:
new_compass_image = self.main_compass_image.rotate(self.current_heading_shown_rotation_angle, resample=PIL.Image.BICUBIC)
self.last_current_heading_shown = self.current_heading_shown_rotation_angle
self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image))
self.show_compass_image__signal.emit()
self.heading_text_update_ready__signal.emit(str(self.current_heading_shown_rotation_angle) + "°")
def update_heading_movement(self):
current_minus_shown = (self.current_heading - self.shown_heading) % 360
if current_minus_shown >= 180:
self.rotation_direction = -1
else:
self.rotation_direction = 1
def on_heading_changed__slot(self, new_heading):
self.current_heading = new_heading
self.heading_text_update_ready__signal.emit(str(new_heading) + "°")
self.current_heading_changed = True
def __on_heading_clicked__slot(self, event):
new_heading = self.current_heading
if event.button() == QtCore.Qt.LeftButton:
new_heading = (self.current_heading + 5) % 360
elif event.button() == QtCore.Qt.RightButton:
new_heading = (self.current_heading - 5) % 360
self.on_heading_changed__slot(new_heading)
self.new_speed_update_ready__signal.emit("%.2f" % (random() * 2.5))
self.heading_text_update_ready__signal.emit(str(new_heading) + "°")
def on_new_compass_image_ready__slot(self):
self.heading_compass_label.setPixmap(self.compass_pixmap)
def on_imu_data_received(self, data):
self.imu_data = data
self.new_imu_data = True
def connect_signals_and_slots(self):
self.show_compass_image__signal.connect(self.on_new_compass_image_ready__slot)
self.heading_text_update_ready__signal.connect(self.heading_text_label.setText)
self.heading_text_update_ready__signal.connect(self.next_goal_label.setText)
self.new_speed_update_ready__signal.connect(self.current_speed_label.setText)
self.heading_compass_label.mousePressEvent = self.__on_heading_clicked__slot
self.pitch_update_ready__signal.connect(self.imu_pitch_lcd_number.display)
self.roll_update_ready__signal.connect(self.imu_roll_lcd_number.display)
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

324
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/NavigationSystems/WaypointsCoordinator.py Normal file
View File

@@ -0,0 +1,324 @@
from PyQt5 import QtCore, QtWidgets, QtGui
import logging
import rospy
from sensor_msgs.msg import NavSatFix
GPS_POSITION_TOPIC = "/rover_odometry/fix"
class WaypointsCoordinator(QtCore.QThread):
new_manual_waypoint_entry = QtCore.pyqtSignal(str, float, float, int)
update_waypoint_entry = QtCore.pyqtSignal(str, str, str, int)
def __init__(self, shared_objects):
super(WaypointsCoordinator, self).__init__()
self.run_thread_flag = True
self.navigation_table_cur_click = None
self.landmark_table_cur_click = None
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.navigation_label = (self.left_screen.
navigation_waypoints_table_widget)
self.landmark_label = self.left_screen.landmark_waypoints_table_widget
self.name_edit_label = self.left_screen.manual_waypoint_landmark_name_line_edit
self.latitude_label = self.left_screen.manual_waypoint_decimal_lattitude_spin_box
self.longitude_label = self.left_screen.manual_waypoint_decimal_longitude_spin_box
self.latitude_degree_label = self.left_screen.manual_waypoint_degrees_lattitude_spin_box
self.longitude_degree_label = self.left_screen.manual_waypoint_degrees_longitude_spin_box
self.latitude_minute_label = self.left_screen.manual_waypoint_minutes_lattitude_spin_box
self.longitude_minute_label = self.left_screen.manual_waypoint_minutes_longitude_spin_box
self.latitude_second_label = self.left_screen.manual_waypoint_seconds_lattitude_spin_box
self.longitude_second_label = self.left_screen.manual_waypoint_seconds_longitude_spin_box
self.latitude_card_label = self.left_screen.manual_waypoint_cardinal_lattitude_combo_box
self.longitude_card_label = self.left_screen.manual_waypoint_cardinal_longitude_combo_box
# Color Labels and Buttons
self.nav_color_label = self.left_screen.manual_waypoint_navigation_color_label
self.nav_color_set_button = self.left_screen.manual_waypoint_navigation_color_set_button
self.landmark_color_label = self.left_screen.manual_waypoint_landmark_color_label
self.landmark_color_set_button = self.left_screen.manual_waypoint_landmark_color_set_button
# Nav Table Buttons
self.nav_set_button_label = (self.left_screen.
navigation_waypoints_set_button)
self.nav_add_manual_button_label = (
self.left_screen.navigation_waypoints_add_manual_button)
self.nav_add_gps_button_label = (self.left_screen.
navigation_waypoints_add_gps_button)
self.nav_delete_button_label = (self.left_screen.
navigation_waypoints_delete_button)
# Land Table Buttons
self.land_set_button_label = (self.left_screen.
landmark_waypoints_set_button)
self.land_add_manual_button_label = (
self.left_screen.landmark_waypoints_add_manual_button)
self.land_add_gps_button_label = (self.left_screen.
landmark_waypoints_add_gps_button)
self.land_delete_button_label = (self.left_screen.
landmark_waypoints_delete_button)
self.settings = QtCore.QSettings()
self.logger = logging.getLogger("groundstation")
self.gps_position_subscriber = rospy.Subscriber(GPS_POSITION_TOPIC, NavSatFix,
self.gps_position_updated_callback)
self.longitude = None
self.latitude = None
self.nav_color = QtCore.Qt.yellow
self.landmark_color = QtCore.Qt.blue
self.nav_color_dialog = QtWidgets.QColorDialog()
self.nav_color_dialog.setWindowFlags(
QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint)
self.landmark_color_dialog = QtWidgets.QColorDialog()
self.landmark_color_dialog.setWindowFlags(
QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint)
def run(self):
while self.run_thread_flag:
self.msleep(3)
def connect_signals_and_slots(self):
self.new_manual_waypoint_entry.connect(self.update_manual_entry)
# setting up signals for Navigation Table
self.nav_add_gps_button_label.clicked.connect(self._nav_add_gps)
self.nav_delete_button_label.clicked.connect(self._nav_del)
self.nav_add_manual_button_label.clicked.connect(self._nav_add_manual)
self.nav_set_button_label.clicked.connect(self._nav_save)
# setting up signals for Landmark Table
self.land_add_gps_button_label.clicked.connect(self._land_add_gps)
self.land_delete_button_label.clicked.connect(self._land_del)
self.land_add_manual_button_label.clicked.connect(self.
_land_add_manual)
self.land_set_button_label.clicked.connect(self._land_save)
self.navigation_label.cellClicked.connect(self._on_nav_clicked)
self.landmark_label.cellClicked.connect(self._on_land_clicked)
self.nav_color_set_button.clicked.connect(self.nav_color_dialog.show)
self.landmark_color_set_button.clicked.connect(self.landmark_color_dialog.show)
self.nav_color_dialog.currentColorChanged.connect(self.__on_new_nav_color_selected)
self.landmark_color_dialog.currentColorChanged.connect(self.__on_new_landmark_color_selected)
def _add_to_table(self, name, lat, lng, color, table):
color_table_item = QtWidgets.QTableWidgetItem()
color_table_item.setBackground(color)
count = table.rowCount()
table.insertRow(count)
table.setItem(count, 0, QtWidgets.QTableWidgetItem(name))
table.setItem(count, 1, QtWidgets.QTableWidgetItem(lat))
table.setItem(count, 2, QtWidgets.QTableWidgetItem(lng))
table.setItem(count, 3, color_table_item)
def _clear_inputs(self):
self.name_edit_label.clear()
self.latitude_label.clear()
self.longitude_label.clear()
def _is_empty_inputs(self):
if not self.name_edit_label.text():
return True
if not self.latitude_label.text():
return True
if not self.longitude_label.text():
return True
return False
def _nav_add_gps(self):
if self.longitude and self.latitude:
name = self.navigation_label.rowCount()
self._add_to_table(str(name + 1), str(self.latitude),
str(self.longitude), self.nav_color,
self.navigation_label)
self._clear_inputs()
def _nav_save(self):
if not self._is_empty_inputs():
lat = self.latitude_label.text()
lng = self.longitude_label.text()
self.navigation_label.setItem(
self.navigation_table_cur_click,
1,
QtWidgets.QTableWidgetItem(lat))
self.navigation_label.setItem(
self.navigation_table_cur_click,
2,
QtWidgets.QTableWidgetItem(lng))
color_table_item = QtWidgets.QTableWidgetItem()
color_table_item.setBackground(self.nav_color)
self.navigation_label.setItem(
self.navigation_table_cur_click,
3,
color_table_item)
self._clear_inputs()
def _nav_add_manual(self):
# request GPS data
if not self._is_empty_inputs():
name = self.navigation_label.rowCount()
lat = self.latitude_label.text()
lng = self.longitude_label.text()
self._add_to_table(str(name + 1), lat,
lng, self.nav_color,
self.navigation_label)
self._clear_inputs()
def _nav_del(self):
if self.navigation_table_cur_click is not None:
self.navigation_label.removeRow(self.navigation_table_cur_click)
count = self.navigation_label.rowCount()
for x in range(self.navigation_table_cur_click, count):
self.navigation_label.setItem(x,
0,
QtWidgets.
QTableWidgetItem(str(x + 1)))
self._clear_inputs()
def _land_add_gps(self):
if self.longitude and self.latitude:
name = self.name_edit_label.text()
self._add_to_table(name, str(self.latitude), str(self.longitude), self.landmark_color, self.landmark_label)
self._clear_inputs()
def _land_add_manual(self):
if not self._is_empty_inputs():
name = self.name_edit_label.text()
lat = self.latitude_label.text()
lng = self.longitude_label.text()
self._add_to_table(name, lat,
lng, self.landmark_color,
self.landmark_label)
self._clear_inputs()
def _land_del(self):
if self.landmark_table_cur_click is not None:
self.landmark_label.removeRow(self.landmark_table_cur_click)
count = self.landmark_label.rowCount()
for x in range(self.landmark_table_cur_click, count):
self.navigation_label.setItem(x,
0,
QtWidgets.
QTableWidgetItem(str(x + 1)))
self._clear_inputs()
def _land_save(self):
if not self._is_empty_inputs():
name = self.name_edit_label.text()
lat = self.latitude_label.text()
lng = self.longitude_label.text()
self.landmark_label.setItem(self.landmark_table_cur_click, 0,
QtWidgets.QTableWidgetItem(name))
self.landmark_label.setItem(self.landmark_table_cur_click, 1,
QtWidgets.QTableWidgetItem(lat))
self.landmark_label.setItem(self.landmark_table_cur_click, 2,
QtWidgets.QTableWidgetItem(lng))
color_table_item = QtWidgets.QTableWidgetItem()
color_table_item.setBackground(self.landmark_color)
self.landmark_label.setItem(self.landmark_table_cur_click, 3,
color_table_item)
self._clear_inputs()
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
def update_manual_entry(self, name, lat, lng, table):
self.name_edit_label.setEnabled(bool(table))
self.name_edit_label.setText(name)
self.latitude_label.setValue(lat)
self.longitude_label.setValue(lng)
lat_d = float(abs(int(lat)))
lat_m = float(int((abs(lat) - lat_d) * 60))
lat_s = ((abs(lat) - lat_d - (lat_m / 60.0)) * 3600.)
if lat > 0.:
self.latitude_card_label.setCurrentText("N")
else:
self.latitude_card_label.setCurrentText("S")
self.latitude_degree_label.setValue(lat_d)
self.latitude_minute_label.setValue(lat_m)
self.latitude_second_label.setValue(lat_s)
lng_d = float(abs(int(lng)))
lng_m = float(int((abs(lng) - lng_d) * 60))
lng_s = ((abs(lng) - lng_d - (lng_m / 60.0)) * 3600.)
if lng > 0.:
self.longitude_card_label.setCurrentText("E")
else:
self.longitude_card_label.setCurrentText("W")
self.longitude_degree_label.setValue(lng_d)
self.longitude_minute_label.setValue(lng_m)
self.longitude_second_label.setValue(lng_s)
def _on_nav_clicked(self, row, col):
self.navigation_table_cur_click = row
self.landmark_table_cur_click = None
self.new_manual_waypoint_entry.emit(
self.navigation_label.item(row, 0).text(),
float(self.navigation_label.item(row, 1).text()),
float(self.navigation_label.item(row, 2).text()),
0
)
def _on_land_clicked(self, row, col):
self.landmark_table_cur_click = row
self.navigation_table_cur_click = None
self.new_manual_waypoint_entry.emit(
self.landmark_label.item(row, 0).text(),
float(self.landmark_label.item(row, 1).text()),
float(self.landmark_label.item(row, 2).text()),
1
)
def __on_new_nav_color_selected(self, color):
self.nav_color_label.setStyleSheet(
"background-color: rgb(%s, %s, %s)" % (color.red(), color.green(), color.blue()))
self.nav_color = color
def __on_new_landmark_color_selected(self, color):
self.landmark_color_label.setStyleSheet(
"background-color: rgb(%s, %s, %s)" % (color.red(), color.green(), color.blue()))
self.landmark_color = color
def gps_position_updated_callback(self, data):
self.latitude = data.latitude
self.longitude = data.longitude

0
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/NavigationSystems/__init__.py Normal file
View File

125
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/SettingsSystems/UbiquitiRadioSettings.py Normal file
View File

@@ -0,0 +1,125 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
from time import time
import paramiko
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 5
ACCESS_POINT_IP = "192.168.1.20" # The channel only has to be set on the access point. The staion will adjust.
ACCESS_POINT_USER = "ubnt"
ACCESS_POINT_PASSWORD = "rover4lyfe^" # We don't care about this password, don't freak out. Wifi is open anyways...
GET_CURRENT_CHANNEL_COMMAND = "iwlist ath0 channel"
SET_CHANNEL_COMMAND = "iwconfig ath0 channel"
#####################################
# UbiquitiRadioSettings Class Definition
#####################################
class UbiquitiRadioSettings(QtCore.QThread):
show_channel__signal = QtCore.pyqtSignal(int)
set_gui_elements_enabled__signal = QtCore.pyqtSignal(bool)
def __init__(self, shared_objects):
super(UbiquitiRadioSettings, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.ubiquiti_channel_spin_box = self.left_screen.ubiquiti_channel_spin_box # type: QtWidgets.QSpinBox
self.ubiquiti_channel_apply_button = self.left_screen.ubiquiti_channel_apply_button # type: QtWidgets.QPushButton
# ########## 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.wait_time = 1.0 / THREAD_HERTZ
self.channel_change_needed = False
self.new_channel = 0
self.ssh_client = None
def run(self):
self.set_gui_elements_enabled__signal.emit(False)
try:
self.setup_and_connect_ssh_client()
except Exception:
return
self.get_and_show_current_channel()
while self.run_thread_flag:
start_time = time()
self.apply_channel_if_needed()
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
def setup_and_connect_ssh_client(self):
self.ssh_client = paramiko.SSHClient()
self.ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
self.ssh_client.connect(ACCESS_POINT_IP, username=ACCESS_POINT_USER, password=ACCESS_POINT_PASSWORD,
compress=True)
def apply_channel_if_needed(self):
if self.channel_change_needed:
self.show_channel__signal.emit(0)
self.set_gui_elements_enabled__signal.emit(False)
self.ssh_client.exec_command(SET_CHANNEL_COMMAND + " %02d" % self.new_channel)
self.get_and_show_current_channel()
self.channel_change_needed = False
def get_and_show_current_channel(self):
channel = 0
ssh_stdin, ssh_stdout, ssh_stderr = self.ssh_client.exec_command(GET_CURRENT_CHANNEL_COMMAND)
output = ssh_stdout.read()
for line in output.split("\n"):
if "Current Frequency:" in line:
channel = line.strip("()").split("Channel ")[1]
break
self.msleep(500) # From the gui, this helps show something is actually happening
self.show_channel__signal.emit(int(channel))
self.set_gui_elements_enabled__signal.emit(True)
def on_ubiquiti_channel_apply_pressed__slot(self):
self.new_channel = self.ubiquiti_channel_spin_box.value()
self.channel_change_needed = True
def connect_signals_and_slots(self):
self.ubiquiti_channel_apply_button.clicked.connect(self.on_ubiquiti_channel_apply_pressed__slot)
self.show_channel__signal.connect(self.ubiquiti_channel_spin_box.setValue)
self.set_gui_elements_enabled__signal.connect(self.ubiquiti_channel_spin_box.setEnabled)
self.set_gui_elements_enabled__signal.connect(self.ubiquiti_channel_apply_button.setEnabled)
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
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/SettingsSystems/__init__.py Normal file
View File

45
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/StartupSystems/ROSMasterChecker.py Normal file
View File

@@ -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
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/StartupSystems/__init__.py Normal file
View File

338
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py Normal file
View File

@@ -0,0 +1,338 @@
#!/usr/bin/env python
# coding=utf-8
import rospy
from rover_status.msg import *
from PyQt5 import QtWidgets, QtCore, QtGui, uic
from std_msgs.msg import Empty
import PIL.Image
from PIL.ImageQt import ImageQt
import time
from std_msgs.msg import UInt16
# import Timer
REQUEST_UPDATE_TOPIC = "/rover_status/update_requested"
CAMERA_TOPIC_NAME = "/rover_status/camera_status"
BOGIE_TOPIC_NAME = "/rover_status/bogie_status"
FRSKY_TOPIC_NAME = "/rover_status/frsky_status"
GPS_TOPIC_NAME = "/rover_status/gps_status"
JETSON_TOPIC_NAME = "/rover_status/jetson_status"
MISC_TOPIC_NAME = "/rover_status/misc_status"
BATTERY_TOPIC_NAME = "/rover_status/battery_status"
CO2_TOPIC_NAME = "/rover_control/tower/status/co2"
COLOR_GREEN = "background-color: darkgreen; border: 1px solid black;"
COLOR_ORANGE = "background-color: orange; border: 1px solid black;"
COLOR_YELLOW = "background-color: rgb(204,204,0); border: 1px solid black; color: black;"
COLOR_RED = "background-color: darkred; border: 1px solid black;"
GPS_BEST_CASE_ACCURACY = 3
LOW_BATTERY_DIALOG_TIMEOUT = 120
CRITICAL_BATTERY_DIALOG_TIMEOUT = 30
class SensorCore(QtCore.QThread):
# ########## create signals for slots ##########
jetson_cpu_update_ready__signal = QtCore.pyqtSignal(str)
jetson_cpu_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
jetson_emmc_update_ready__signal = QtCore.pyqtSignal(str)
jetson_emmc_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
jetson_gpu_temp_update_ready__signal = QtCore.pyqtSignal(str)
jetson_gpu_temp_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
jetson_ram_update_ready__signal = QtCore.pyqtSignal(str)
jetson_ram_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
bogie_connection_1_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
bogie_connection_2_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
bogie_connection_3_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
gps_fix_update_ready__signal = QtCore.pyqtSignal(str)
gps_heading_valid_update_ready__signal = QtCore.pyqtSignal(str)
gps_num_satellites_update_ready__signal = QtCore.pyqtSignal(str)
gps_accuracy_update_ready__signal = QtCore.pyqtSignal(str)
co2_levels_update_ready__signal = QtCore.pyqtSignal(str)
camera_zed_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
camera_under_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
camera_chassis_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
camera_main_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
gps_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
frsky_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
battery_voltage_update_ready__signal = QtCore.pyqtSignal(str)
battery_voltage_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
show_battery_low__signal = QtCore.pyqtSignal()
show_battery_critical__signal = QtCore.pyqtSignal()
def __init__(self, shared_objects):
super(SensorCore, self).__init__()
self.run_thread_flag = True
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.screen_main_window = self.shared_objects["screens"]["left_screen"]
# self.cpu_read = self.screen_main_window.lineEdit # type: QtWidgets.QLabel
# self.ram_read = self.screen_main_window.lineEdit_2 # type: QtWidgets.QLabel
# ########## set vars to gui elements
self.osurc_logo_label = self.screen_main_window.osurc_logo_label # type: QtWidgets.QLabel
self.rover_conn = self.screen_main_window.rover # type: QtWidgets.QLabel
self.frsky = self.screen_main_window.frsky # type: QtWidgets.QLabel
self.nav_mouse = self.screen_main_window.nav_mouse # type: QtWidgets.QLabel
self.joystick = self.screen_main_window.joystick # type: QtWidgets.QLabel
self.gps_fix_label = self.screen_main_window.gps_fix_label # type: QtWidgets.QLabel
self.gps_heading_valid_label = self.screen_main_window.gps_heading_valid_label # type: QtWidgets.QLabel
self.gps_num_satellites_label = self.screen_main_window.gps_num_satellites_label # type: QtWidgets.QLabel
self.gps_accuracy_label = self.screen_main_window.gps_accuracy_label # type: QtWidgets.QLabel
self.zed = self.screen_main_window.zed # type: QtWidgets.QLabel
self.main_cam = self.screen_main_window.main_cam # type: QtWidgets.QLabel
self.chassis_cam = self.screen_main_window.chassis_cam # type: QtWidgets.QLabel
self.under_cam = self.screen_main_window.under_cam # type: QtWidgets.QLabel
self.clock = self.screen_main_window.clock_qlcdnumber # type: QtWidgets.QLCDNumber
self.cpu = self.screen_main_window.cpu # type: QtWidgets.QLabel
self.ram = self.screen_main_window.ram # type: QtWidgets.QLabel
self.gpu_temp = self.screen_main_window.gpu_temp # type: QtWidgets.QLabel
self.emmc = self.screen_main_window.emmc # type: QtWidgets.QLabel
self.battery = self.screen_main_window.battery_voltage_status_label # type: QtWidgets.QLabel
self.co2_levels_label = self.screen_main_window.co2_levels_label # type: QtWidgets.QLabel
# ########## subscriptions pulling data from system_statuses_node.py ##########
self.camera_status = rospy.Subscriber(CAMERA_TOPIC_NAME, CameraStatuses, self.__camera_callback)
self.frsky_status = rospy.Subscriber(FRSKY_TOPIC_NAME, FrSkyStatus, self.__frsky_callback)
self.gps_status = rospy.Subscriber(GPS_TOPIC_NAME, GPSInfo, self.__gps_callback)
self.jetson_status = rospy.Subscriber(JETSON_TOPIC_NAME, JetsonInfo, self.__jetson_callback)
self.misc_status = rospy.Subscriber(MISC_TOPIC_NAME, MiscStatuses, self.__misc_callback)
self.battery_status = rospy.Subscriber(BATTERY_TOPIC_NAME, BatteryStatusMessage, self.__battery_callback)
self.co2_status = rospy.Subscriber(CO2_TOPIC_NAME, UInt16, self.__co2_callback)
self.camera_msg = CameraStatuses()
self.bogie_msg = None # BogieStatuses()
self.FrSky_msg = FrSkyStatus()
self.GPS_msg = GPSInfo()
self.jetson_msg = JetsonInfo()
self.misc_msg = MiscStatuses()
self.battery_msg = BatteryStatusMessage()
self.update_requester = rospy.Publisher(REQUEST_UPDATE_TOPIC, Empty, queue_size=10)
# Apply OSURC Logo
self.osurc_logo_pil = PIL.Image.open("Resources/Images/osurclogo.png").resize((210, 75), PIL.Image.BICUBIC)
self.osurc_logo_pixmap = QtGui.QPixmap.fromImage(ImageQt(self.osurc_logo_pil))
self.osurc_logo_label.setPixmap(self.osurc_logo_pixmap) # Init should be in main thread, should be fine
self.low_battery_warning_dialog = QtWidgets.QMessageBox()
self.low_battery_warning_dialog.setIcon(QtWidgets.QMessageBox.Warning)
self.low_battery_warning_dialog.setText("\n\n\n\nRover battery low!\nReturn and charge soon to avoid battery damage!\n\n\n\n")
self.low_battery_warning_dialog.setWindowTitle("Low Battery")
self.low_battery_warning_dialog.setStandardButtons(QtWidgets.QMessageBox.Ok)
self.low_battery_warning_dialog.setWindowFlags(
QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint)
self.low_battery_warning_dialog.setStyleSheet(COLOR_YELLOW)
self.critical_battery_warning_dialog = QtWidgets.QMessageBox()
self.critical_battery_warning_dialog.setIcon(QtWidgets.QMessageBox.Critical)
self.critical_battery_warning_dialog.setText(
"\n\n\n\nRover battery critical!\nPower down immediately or battery damage will occur!\n\n\n\n")
self.critical_battery_warning_dialog.setWindowTitle("Critical Battery")
self.critical_battery_warning_dialog.setStandardButtons(QtWidgets.QMessageBox.Ok)
self.critical_battery_warning_dialog.setWindowFlags(
QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint)
self.critical_battery_warning_dialog.setStyleSheet(COLOR_RED)
self.low_battery_warning_last_shown = 0
self.critical_battery_warning_last_shown = 0
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
if data.camera_zed is False:
# self.zed.setStyleSheet("background-color: red;")
self.camera_zed_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
# self.zed.setStyleSheet(COLOR_GREEN)
self.camera_zed_stylesheet_change_ready__signal.emit(COLOR_GREEN)
if data.camera_undercarriage is False:
# self.under_cam.setStyleSheet(COLOR_RED)
self.camera_under_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
# self.under_cam.setStyleSheet(COLOR_GREEN)
self.camera_under_stylesheet_change_ready__signal.emit(COLOR_GREEN)
if data.camera_chassis is False:
# self.chassis_cam.setStyleSheet(COLOR_RED)
self.camera_chassis_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
# self.chassis_cam.setStyleSheet(COLOR_GREEN)
self.camera_chassis_stylesheet_change_ready__signal.emit(COLOR_GREEN)
if data.camera_main_navigation is False:
# self.main_cam.setStyleSheet(COLOR_RED)
self.camera_main_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
# self.main_cam.setStyleSheet(COLOR_GREEN)
self.camera_main_stylesheet_change_ready__signal.emit(COLOR_GREEN)
def __frsky_callback(self, data):
self.FrSky_msg.FrSky_controller_connection_status = data.FrSky_controller_connection_status
if self.FrSky_msg.FrSky_controller_connection_status is False:
self.frsky_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
self.frsky_stylesheet_change_ready__signal.emit(COLOR_GREEN)
def __jetson_callback(self, data):
self.jetson_cpu_update_ready__signal.emit("TX2 CPU\n" + str(data.jetson_CPU) + " %")
if data.jetson_CPU > 85:
self.jetson_cpu_stylesheet_change_ready__signal.emit(COLOR_ORANGE)
elif data.jetson_CPU > 95:
self.jetson_cpu_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
self.jetson_cpu_stylesheet_change_ready__signal.emit(COLOR_GREEN)
self.jetson_ram_update_ready__signal.emit("TX2 RAM\n" + str(data.jetson_RAM) + " %")
if data.jetson_RAM > 79:
self.jetson_ram_stylesheet_change_ready__signal.emit(COLOR_ORANGE)
elif data.jetson_RAM > 89:
self.jetson_ram_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
self.jetson_ram_stylesheet_change_ready__signal.emit(COLOR_GREEN)
self.jetson_gpu_temp_update_ready__signal.emit("TX2 TEMP\n" + str(data.jetson_GPU_temp) + " °C")
if data.jetson_GPU_temp > 64:
self.jetson_gpu_temp_stylesheet_change_ready__signal.emit(COLOR_ORANGE)
elif data.jetson_GPU_temp > 79:
self.jetson_gpu_temp_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
self.jetson_gpu_temp_stylesheet_change_ready__signal.emit(COLOR_GREEN)
self.jetson_emmc_update_ready__signal.emit("TX2 EMMC\n" + str(data.jetson_EMMC) + " %")
if data.jetson_EMMC > 79:
self.jetson_emmc_stylesheet_change_ready__signal.emit(COLOR_ORANGE)
elif data.jetson_EMMC > 89:
self.jetson_emmc_stylesheet_change_ready__signal.emit(COLOR_RED)
else:
self.jetson_emmc_stylesheet_change_ready__signal.emit(COLOR_GREEN)
def __gps_callback(self, data):
if data.gps_connected:
self.gps_fix_update_ready__signal.emit("GPS Fix\nTrue")
self.gps_stylesheet_change_ready__signal.emit(COLOR_GREEN)
else:
self.gps_stylesheet_change_ready__signal.emit(COLOR_RED)
self.gps_fix_update_ready__signal.emit("GPS Fix\nFalse")
if data.gps_heading != -1:
self.gps_heading_valid_update_ready__signal.emit("GPS Heading Valid\nTrue")
else:
self.gps_heading_valid_update_ready__signal.emit("GPS Heading Valid\nFalse")
self.gps_num_satellites_update_ready__signal.emit("GPS Satellites\n%s" % data.num_satellites)
self.gps_accuracy_update_ready__signal.emit(
"GPS Accuracy\n%2.2f m" % (data.horizontal_dilution * GPS_BEST_CASE_ACCURACY))
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 __battery_callback(self, data):
voltage = data.battery_voltage / 1000.0
if voltage >= 21:
self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_GREEN)
elif voltage >= 19:
self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_YELLOW)
if (time.time() - self.low_battery_warning_last_shown) > LOW_BATTERY_DIALOG_TIMEOUT:
self.show_battery_low__signal.emit()
self.low_battery_warning_last_shown = time.time()
else:
self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_RED)
if (time.time() - self.critical_battery_warning_last_shown) > CRITICAL_BATTERY_DIALOG_TIMEOUT:
self.show_battery_critical__signal.emit()
self.critical_battery_warning_last_shown = time.time()
self.battery_voltage_update_ready__signal.emit("Battery Voltage\n" + str(voltage) + " V")
def __co2_callback(self, data):
if data.data != 9999:
self.co2_levels_update_ready__signal.emit("CO2 Levels\n%d ppm" % data.data)
else:
self.co2_levels_update_ready__signal.emit("CO2 Levels\n--- ppm")
def __display_time(self):
time = QtCore.QTime.currentTime()
temp = time.toString('hh:mm')
self.clock.display(temp)
def run(self):
while self.run_thread_flag:
# self.update_requester.publish(Empty())
self.__display_time()
self.msleep(1000)
def connect_signals_and_slots(self):
self.jetson_cpu_update_ready__signal.connect(self.cpu.setText)
self.jetson_cpu_stylesheet_change_ready__signal.connect(self.cpu.setStyleSheet)
self.jetson_ram_update_ready__signal.connect(self.ram.setText)
self.jetson_ram_stylesheet_change_ready__signal.connect(self.ram.setStyleSheet)
self.jetson_emmc_update_ready__signal.connect(self.emmc.setText)
self.jetson_emmc_stylesheet_change_ready__signal.connect(self.emmc.setStyleSheet)
self.jetson_gpu_temp_update_ready__signal.connect(self.gpu_temp.setText)
self.jetson_gpu_temp_stylesheet_change_ready__signal.connect(self.gpu_temp.setStyleSheet)
self.camera_zed_stylesheet_change_ready__signal.connect(self.zed.setStyleSheet)
self.camera_under_stylesheet_change_ready__signal.connect(self.under_cam.setStyleSheet)
self.camera_chassis_stylesheet_change_ready__signal.connect(self.chassis_cam.setStyleSheet)
self.camera_main_stylesheet_change_ready__signal.connect(self.main_cam.setStyleSheet)
self.gps_stylesheet_change_ready__signal.connect(self.gps_fix_label.setStyleSheet)
self.frsky_stylesheet_change_ready__signal.connect(self.frsky.setStyleSheet)
self.gps_fix_update_ready__signal.connect(self.gps_fix_label.setText)
self.gps_heading_valid_update_ready__signal.connect(self.gps_heading_valid_label.setText)
self.gps_num_satellites_update_ready__signal.connect(self.gps_num_satellites_label.setText)
self.gps_accuracy_update_ready__signal.connect(self.gps_accuracy_label.setText)
self.co2_levels_update_ready__signal.connect(self.co2_levels_label.setText)
self.battery_voltage_update_ready__signal.connect(self.battery.setText)
self.battery_voltage_stylesheet_change_ready__signal.connect(self.battery.setStyleSheet)
self.show_battery_low__signal.connect(self.low_battery_warning_dialog.show)
self.show_battery_critical__signal.connect(self.critical_battery_warning_dialog.show)
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
if __name__ == '__main__':
rover_statuses = SensorCore()
rover_statuses.run()

70
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/StatusSystems/Timer.py Normal file
View File

@@ -0,0 +1,70 @@
#!/usr/bin/env python2
import sys
from PyQt5 import Qt
from PyQt5 import QtCore
from PyQt5.uic import loadUi
# [ms]
TICK_TIME = 2**6
class Timer(Qt.QMainWindow):
def __init__(self):
super(Timer, self).__init__()
# self.reset.clicked.connect(self.do_reset)
# self.start.clicked.connect(self.do_start)
self.timer = Qt.QTimer()
self.timer.setInterval(TICK_TIME)
self.timer.timeout.connect(self.tick)
self.running = False;
self.do_reset()
def __start_stop_reset_event(self, event):
if event.button() == QtCore.Qt.LeftButton:
if self.running is False:
self.do_start
elif self.running is True:
self.do_pause
elif event.button() == QtCore.Qt.RightButton:
self.do_reset
def display(self):
self.timer.display("%d:%05.2f" % (self.time // 60, self.time % 60))
@Qt.pyqtSlot()
def tick(self):
self.time += TICK_TIME/1000
self.display()
@Qt.pyqtSlot()
def do_start(self):
self.timer.start()
# self.start.setText("Pause")
# self.start.clicked.disconnect()
# self.start.clicked.connect(self.do_pause)
@Qt.pyqtSlot()
def do_pause(self):
self.timer.stop()
# self.start.setText("Start")
# self.start.clicked.disconnect()
# self.start.clicked.connect(self.do_start)
@Qt.pyqtSlot()
def do_reset(self):
self.time = 0
self.display()
app = Qt.QApplication(sys.argv)
timer = Timer()
timer.show()
app.exec_()

126
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/StatusSystems/UbiquitiStatusCore.py Normal file
View File

@@ -0,0 +1,126 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
from time import time
import paramiko
from pprint import pprint
import json
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 5
ACCESS_POINT_IP = "192.168.1.20" # The channel only has to be set on the access point. The staion will adjust.
ACCESS_POINT_USER = "ubnt"
ACCESS_POINT_PASSWORD = "rover4lyfe^" # We don't care about this password, don't freak out. Wifi is open anyways...
GENERAL_WIRELESS_INFO_COMMAND = "wstalist"
#####################################
# UbiquitiRadioSettings Class Definition
#####################################
class UbiquitiStatus(QtCore.QThread):
connection_quality_update_ready__signal = QtCore.pyqtSignal(str)
successful_transmission_update_ready__signal = QtCore.pyqtSignal(str)
radio_rates_update_ready__signal = QtCore.pyqtSignal(str)
radio_latency_update_ready__signal = QtCore.pyqtSignal(str)
def __init__(self, shared_objects):
super(UbiquitiStatus, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.connection_quality_label = self.left_screen.connection_quality_label
self.successful_transmit_label = self.left_screen.successful_transmit_label
self.radio_rates_label = self.left_screen.radio_rates_label
self.radio_latency_label = self.left_screen.radio_latency_label
# ########## 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.wait_time = 1.0 / THREAD_HERTZ
self.ssh_client = None
def run(self):
try:
self.setup_and_connect_ssh_client()
except Exception:
return
while self.run_thread_flag:
start_time = time()
try:
self.get_and_show_ubiquiti_status()
except Exception, e:
print e
time_diff = time() - start_time
self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
def setup_and_connect_ssh_client(self):
self.ssh_client = paramiko.SSHClient()
self.ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
self.ssh_client.connect(ACCESS_POINT_IP, username=ACCESS_POINT_USER, password=ACCESS_POINT_PASSWORD,
compress=True)
def get_and_show_ubiquiti_status(self):
ssh_stdin, ssh_stdout, ssh_stderr = self.ssh_client.exec_command(GENERAL_WIRELESS_INFO_COMMAND)
try:
output_json = json.loads(ssh_stdout.read())[0]
transmit_percent = output_json["ccq"]
quality = output_json["airmax"]["quality"]
# capacity = output_json["airmax"]["capacity"]
rx_rate = output_json["rx"]
tx_rate = output_json["tx"]
ground_tx_latency = output_json["tx_latency"]
rover_tx_latency = output_json["remote"]["tx_latency"]
except IndexError:
transmit_percent = 0
quality = 0
# capacity = output_json["airmax"]["capacity"]
rx_rate = 0
tx_rate = 0
ground_tx_latency = "----"
rover_tx_latency = "----"
self.connection_quality_update_ready__signal.emit("Connection Quality\n%s %%" % quality)
self.successful_transmission_update_ready__signal.emit("Successful Transmit\n%s %%" % transmit_percent)
self.radio_rates_update_ready__signal.emit("TX Rate: %s Mbps\nRX Rate: %s Mbps" % (tx_rate, rx_rate))
self.radio_latency_update_ready__signal.emit(
"TX Latency: %s ms\nRX Latency: %s ms" % (ground_tx_latency, rover_tx_latency))
def connect_signals_and_slots(self):
self.connection_quality_update_ready__signal.connect(self.connection_quality_label.setText)
self.successful_transmission_update_ready__signal.connect(self.successful_transmit_label.setText)
self.radio_rates_update_ready__signal.connect(self.radio_rates_label.setText)
self.radio_latency_update_ready__signal.connect(self.radio_latency_label.setText)
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
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/StatusSystems/__init__.py Normal file
View File

391
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py Normal file
View File

@@ -0,0 +1,391 @@
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
from time import time
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 = (640, 360)
TERTIARY_LABEL_MAX = (640, 360)
LOW_RES = (256, 144)
GUI_SELECTION_CHANGE_TIMEOUT = 3 # Seconds
STYLESHEET_SELECTED = "border: 2px solid orange; background-color:black;"
STYLESHEET_UNSELECTED = "background-color:black;"
COLOR_GREEN = "background-color: darkgreen;"
COLOR_RED = "background-color: darkred;"
#####################################
# RoverVideoCoordinator Class Definition
#####################################
class RoverVideoCoordinator(QtCore.QThread):
pixmap_ready_signal = QtCore.pyqtSignal(str)
update_element_stylesheet__signal = QtCore.pyqtSignal()
pan_tilt_selection_changed__signal = QtCore.pyqtSignal(str)
low_res_button_text_update_ready__signal = QtCore.pyqtSignal(str)
low_res_button_stylesheet_update_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
self.low_res_mode_button = self.right_screen.low_res_mode_button # type: QtWidgets.QLabel
self.index_to_label_element = {
0: self.primary_video_display_label,
1: self.secondary_video_display_label,
2: self.tertiary_video_display_label
}
# ########## 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 = []
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.main_nav_index = -1
self.chassis_index = -1
self.__get_cameras()
self.__setup_video_threads()
self.primary_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.index_to_label_current_setting = {
0: self.primary_label_current_setting,
1: self.secondary_label_current_setting,
2: self.tertiary_label_current_setting
}
self.current_label_for_joystick_adjust = 0
self.gui_selection_changed = True
self.last_gui_selection_changed_time = time()
self.set_max_resolutions_flag = True
self.first_image_received = False
self.in_low_res_mode = False
def run(self):
self.logger.debug("Starting Video Coordinator Thread")
self.__broadcast_current_pan_tilt_selection()
while self.run_thread_flag:
self.__set_max_resolutions()
self.__toggle_background_cameras_if_needed()
self.__update_gui_element_selection()
self.msleep(10)
self.__wait_for_camera_threads()
self.logger.debug("Stopping Video Coordinator Thread")
def __broadcast_current_pan_tilt_selection(self):
setting = None
if self.current_label_for_joystick_adjust == 0: # primary
setting = self.primary_label_current_setting
elif self.current_label_for_joystick_adjust == 1: # secondary
setting = self.secondary_label_current_setting
elif self.current_label_for_joystick_adjust == 2: # tertiary
setting = self.tertiary_label_current_setting
if setting == self.main_nav_index:
self.pan_tilt_selection_changed__signal.emit("tower_pan_tilt")
elif setting == self.chassis_index:
self.pan_tilt_selection_changed__signal.emit("chassis_pan_tilt")
else:
self.pan_tilt_selection_changed__signal.emit("no_pan_tilt")
def __set_max_resolutions(self):
if self.set_max_resolutions_flag:
if self.in_low_res_mode:
for camera in self.camera_threads:
self.camera_threads[camera].set_hard_max_resolution(LOW_RES)
else:
self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].set_hard_max_resolution(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.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)
self.set_max_resolutions_flag = False
def __toggle_background_cameras_if_needed(self):
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 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 __update_gui_element_selection(self):
if (time() - self.last_gui_selection_changed_time) > GUI_SELECTION_CHANGE_TIMEOUT \
and self.gui_selection_changed:
elements_to_reset = range(len(self.index_to_label_element))
for index in elements_to_reset:
self.index_to_label_element[index].setStyleSheet(STYLESHEET_UNSELECTED)
self.gui_selection_changed = False
def __on_gui_element_stylesheet_update__slot(self):
elements_to_reset = range(len(self.index_to_label_element))
elements_to_reset.remove(self.current_label_for_joystick_adjust)
for index in elements_to_reset:
self.index_to_label_element[index].setStyleSheet(STYLESHEET_UNSELECTED)
self.index_to_label_element[self.current_label_for_joystick_adjust].setStyleSheet(STYLESHEET_SELECTED)
self.gui_selection_changed = True
self.last_gui_selection_changed_time = time()
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 = []
current_count = 0
if "main_navigation" in names:
self.valid_cameras.append("main_navigation")
self.main_nav_index = current_count
current_count += 1
if "chassis" in names:
self.valid_cameras.append("chassis")
self.chassis_index = current_count
if "undercarriage" in names:
self.valid_cameras.append("undercarriage")
if "end_effector" in names:
self.valid_cameras.append("end_effector")
def __setup_video_threads(self):
for camera in self.valid_cameras:
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
self.shared_objects["threaded_classes"]["Joystick Sender"].change_gui_element_selection__signal.connect(
self.on_camera_gui_element_selection_changed)
self.shared_objects["threaded_classes"]["Joystick Sender"].change_camera_selection__signal.connect(
self.on_camera_selection_for_current_gui_element_changed)
self.shared_objects["threaded_classes"]["Joystick Sender"].toggle_selected_gui_camera__signal.connect(
self.on_gui_selected_camera_toggled)
self.low_res_mode_button.clicked.connect(self.on_low_res_button_clicked__slot)
self.low_res_button_text_update_ready__signal.connect(self.low_res_mode_button.setText)
self.low_res_button_stylesheet_update_ready__signal.connect(self.low_res_mode_button.setStyleSheet)
self.update_element_stylesheet__signal.connect(self.__on_gui_element_stylesheet_update__slot)
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)
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 self.valid_cameras[self.primary_label_current_setting] == camera:
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:
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:
try:
self.tertiary_video_display_label.setPixmap(self.camera_threads[camera].pixmap_640x360_image)
except:
pass
def on_camera_gui_element_selection_changed(self, direction):
new_selection = self.current_label_for_joystick_adjust - direction
if new_selection < 0:
self.current_label_for_joystick_adjust = len(self.index_to_label_element) - 1
elif new_selection == len(self.index_to_label_element):
self.current_label_for_joystick_adjust = 0
else:
self.current_label_for_joystick_adjust = new_selection
self.__broadcast_current_pan_tilt_selection()
self.update_element_stylesheet__signal.emit()
def on_camera_selection_for_current_gui_element_changed(self, direction):
if self.current_label_for_joystick_adjust == 0: # primary
self.primary_label_current_setting = (self.primary_label_current_setting + direction) % len(self.valid_cameras)
elif self.current_label_for_joystick_adjust == 1: # secondary
self.secondary_label_current_setting = (self.secondary_label_current_setting + direction) % len(self.valid_cameras)
elif self.current_label_for_joystick_adjust == 2: # tertiary
self.tertiary_label_current_setting = (self.tertiary_label_current_setting + direction) % len(self.valid_cameras)
self.__broadcast_current_pan_tilt_selection()
self.set_max_resolutions_flag = True
self.update_element_stylesheet__signal.emit()
def on_gui_selected_camera_toggled(self):
if self.current_label_for_joystick_adjust == 0: # primary
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()
elif self.current_label_for_joystick_adjust == 1: # secondary
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()
elif self.current_label_for_joystick_adjust == 2: # tertiary
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()
self.update_element_stylesheet__signal.emit()
def on_low_res_button_clicked__slot(self):
if self.low_res_mode_button.text() == "ENABLED":
self.in_low_res_mode = False
self.set_max_resolutions_flag = True
self.low_res_button_text_update_ready__signal.emit("DISABLED")
self.low_res_button_stylesheet_update_ready__signal.emit(COLOR_GREEN)
else:
self.in_low_res_mode = True
self.set_max_resolutions_flag = True
self.low_res_button_text_update_ready__signal.emit("ENABLED")
self.low_res_button_stylesheet_update_ready__signal.emit(COLOR_RED)
def on_kill_threads_requested__slot(self):
self.run_thread_flag = False

268
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiver.py Normal file
View File

@@ -0,0 +1,268 @@
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtGui, QtWidgets
import logging
import cv2
import numpy as np
import qimage2ndarray
from time import time
import rospy
import dynamic_reconfigure.client
from cv_bridge import CvBridge
from sensor_msgs.msg import CompressedImage
# Custom Imports
from rover_camera.msg import CameraControlMessage
#####################################
# Global Variables
#####################################
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
#####################################
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__()
# ########## 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.control_topic_path = self.topic_base_path + "/camera_control"
# Subscription variables
self.video_subscribers = []
# 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
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
# Initial class setup to make text images and get camera resolutions
self.__create_camera_name_opencv_images()
# Attach subscribers now that everything is set up
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))
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()
else:
self.__show_video_disabled()
self.msleep(10)
self.logger.debug("Stopping \"%s\" Camera Thread" % self.camera_title_name)
def __enable_camera_resolution(self, resolution):
self.camera_control_publisher.publish(self.RESOLUTION_MAPPINGS[resolution])
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
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)
else:
self.current_resolution_index = min(self.current_resolution_index, self.max_resolution_index)
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
# print "%s: %s FPS" % (self.camera_title_name, current_fps)
self.last_framerate_time = time()
self.frame_count = 0
def __show_video_enabled(self):
if self.new_frame:
self.__check_framerate_and_adjust_resolution()
try:
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)
except Exception, error:
print "Failed with error:" + str(error)
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.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]
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 set_hard_max_resolution(self, resolution):
self.max_resolution_index = self.RESOLUTION_OPTIONS.index(resolution)
def toggle_video_display(self):
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
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
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/VideoSystems/__init__.py Normal file
View File

0
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Framework/__init__.py Normal file
View File

14
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Readme.md Normal file
View 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

BIN
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Images/compass.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 233 KiB

BIN
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Images/maps_loading.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.0 MiB

BIN
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Images/osurclogo.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 22 KiB

BIN
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Images/rover.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 220 KiB

66
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py Normal file
View File

@@ -0,0 +1,66 @@
# Note that the lat and lon positions below correspond to the center point of the maps you want to download
# Proper zoom level selection determines total viewable area
MAPPING_LOCATIONS = {
"Graf Hall": {
"latitude": 44.5675721667,
"longitude": -123.2750535,
"default_zoom": 18,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Crystal Lake": {
"latitude": 44.547155,
"longitude": -123.251438,
"default_zoom": 18,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Bend's \"The Pit\"": {
"latitude": 43.9941317,
"longitude": -121.4150066,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"McMullen": {
"latitude": 51.470326,
"longitude": -112.773995,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Compound": {
"latitude": 51.470941,
"longitude": -112.752322,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"1st Avenue": {
"latitude": 51.453744,
"longitude": -112.715879,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Rosedale": {
"latitude": 51.421368,
"longitude": -112.641666,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
}
LAST_SELECTION = "Graf Hall"
LAST_ZOOM_LEVEL = MAPPING_LOCATIONS[LAST_SELECTION]["default_zoom"]
# ##### This is the offset from magnetic north to true north
DECLINATION_OFFSET = 0 # We set this to 0 so we can use a phone to calibrate

0
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Settings/__init__.py Normal file
View File

4092
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui Normal file
View File

File diff suppressed because it is too large Load Diff

2734
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui Normal file
View File

File diff suppressed because it is too large Load Diff

2439
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/Ui/right_screen_old.ui Normal file
View File

File diff suppressed because it is too large Load Diff

0
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/Resources/__init__.py Normal file
View File

207
OSU Robotics Club/Mars Rover 2017-2018/software/ros_packages/ground_station/src/ground_station.py Normal file
View File

@@ -0,0 +1,207 @@
#####################################
# Imports
#####################################
# Python native imports
import sys
from PyQt5 import QtWidgets, QtCore, QtGui, uic
import signal
import rospy
import logging
import qdarkstyle
# 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.ControlSystems.DriveAndCameraControlSender as JoystickControlSender
import Framework.ControlSystems.EffectorsAndArmControlSender as ControllerControlSender
import Framework.NavigationSystems.SpeedAndHeadingIndication as SpeedAndHeading
import Framework.NavigationSystems.WaypointsCoordinator as WaypointsCoordinator
import Framework.ArmSystems.ArmIndication as ArmIndication
import Framework.StatusSystems.StatusCore as StatusCore
import Framework.StatusSystems.UbiquitiStatusCore as UbiquitiStatusCore
import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings
import Framework.MiscSystems.MiningCore as MiningCore
import Framework.MiscSystems.BashConsoleCore as BashConsoleCore
import Framework.MiscSystems.MiscArmCore as MiscArmCore
import Framework.MiscSystems.RDFCore as RDFCore
#####################################
# 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 ######
self.__add_non_thread("Mining System", MiningCore.Mining(self.shared_objects))
self.__add_non_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects))
# ##### 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("Joystick Sender", JoystickControlSender.DriveAndCameraControlSender(self.shared_objects))
self.__add_thread("Controller Sender", ControllerControlSender.EffectorsAndArmControlSender(self.shared_objects))
self.__add_thread("Speed and Heading", SpeedAndHeading.SpeedAndHeadingIndication(self.shared_objects))
self.__add_thread("Rover Status", StatusCore.SensorCore(self.shared_objects))
self.__add_thread("Ubiquiti Status", UbiquitiStatusCore.UbiquitiStatus(self.shared_objects))
self.__add_thread("Ubiquiti Radio Settings", UbiquitiRadioSettings.UbiquitiRadioSettings(self.shared_objects))
self.__add_thread("Waypoints Coordinator", WaypointsCoordinator.WaypointsCoordinator(self.shared_objects))
self.__add_thread("Bash Console", BashConsoleCore.BashConsole(self.shared_objects))
self.__add_thread("Misc Arm", MiscArmCore.MiscArm(self.shared_objects))
self.__add_thread("RDF", RDFCore.RDF(self.shared_objects))
self.connect_signals_and_slots_signal.emit()
self.__connect_signals_to_slots()
self.start_threads_signal.emit()
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 __add_non_thread(self, name, instance):
self.shared_objects["regular_classes"][name] = instance
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