Added applied robotics code to archive.

OSU Coursework/ROB 421 - Applied Robotics/software/denso_setup.sh

@@ -0,0 +1,50 @@
+#!/usr/bin/env bash
+# As a point of reference, the environment layout should be as follows
+# /home/$user/Github/Rover_2017_2018 for the OSURC github repo
+# /home/$user/catkin_workspace for the user's catkin catkin_workspace
+# By keeping this consistent across all development machines, it will make it
+# easier to keep track of things
+
+# Which folders should be symbolically_linked?
+folders_to_link=(
+    denso_main
+    denso_master
+    zed_wrapper
+    denso_interface_controller
+)
+
+# Print heading
+echo "Setting up ROS packages for denso."
+
+# Get the catkin_workspace directory
+catkin_workspace_dir="catkin_workspace"
+catkin_workspace_path="$HOME/$catkin_workspace_dir"
+catkin_src_path="$catkin_workspace_path/src"
+
+# Get the rover software directory
+github_rover_repo_dir="Github/team17_applied_robotics"
+github_rover_packages_path="$HOME/$github_rover_repo_dir/software/ros_packages"
+
+# Remove existing symbolic links if necessary
+symlinked_folders=$(find $catkin_src_path -maxdepth 1 -type l)
+if [ -z $symlinked_folders ]; then
+    echo "No symlinks to remove from catkin_workspace. Skipping."
+else
+    echo "Removing existing symlinks in catkin_workspace."
+    rm $symlinked_folders
+fi
+
+# Make the new symbolic link connections
+echo "Making new symlinks."
+for folder in ${folders_to_link[@]}; do
+    ln -s "$github_rover_packages_path/$folder" "$catkin_src_path/."
+    echo "Adding symlink for $folder."
+done
+
+# catkin_make so the new pacakges are available and re-source bash
+cd "$catkin_workspace_path"
+catkin_make
+
+source ~/.bashrc
+
+exit 0

OSU Coursework/ROB 421 - Applied Robotics/software/environment/denso_shutdown.sh

@@ -0,0 +1,12 @@
+#!/bin/bash
+
+VBoxManage controlvm Windows acpipowerbutton
+
+echo "Waiting for Windows to poweroff..."
+
+until $(VBoxManage showvminfo --machinereadable Windows | grep -q ^VMState=.poweroff.)
+do
+  sleep 1
+done
+
+sudo poweroff

OSU Coursework/ROB 421 - Applied Robotics/software/environment/denso_startup.sh

@@ -0,0 +1,14 @@
+#!/bin/bash
+
+until ls /dev/vboxdrv
+do
+    echo "Waiting for /dev/vboxdrv"
+    sleep 1
+done
+
+VBoxManage startvm Windows --type headless
+
+source /opt/ros/kinetic/setup.bash
+source /home/denso/catkin_workspace/devel/setup.bash
+
+roslaunch denso_main denso.launch

OSU Coursework/ROB 421 - Applied Robotics/software/firmware/interface_controller/interface_controller.ino

@@ -0,0 +1,192 @@
+#include <Servo.h>4
+
+////////// Pinout Variables //////////
+const int led_data_pin = 9;
+const int led2_data_pin = 8;
+const int shooting_servo_pin = 11;
+const int tamping_servo_pin = 10;
+const int relay_control_pin = 3;
+const int pressure_sensor_pin = A0;
+
+const int denso_sense_pin_2 = 4;
+const int denso_sense_pin_3 = 5;
+
+const int ball_sense_pwm_pin = 7;
+const int ball_sense_feedback_pin = 6;
+
+////////// Global Variables //////////
+String read_string = "";
+
+const int safety_psi_max = 80;
+float current_set_pressure = 0;
+float current_actual_pressure = 0;
+bool set_pressure_reached = false;
+float allowed_psi_error = 4.0;
+
+bool should_tamp = false;
+bool should_shoot = false;
+
+Servo firing_servo;
+Servo tamping_servo;
+
+void setup() {
+  Serial.begin(57600);
+
+  pinMode(pressure_sensor_pin, INPUT);
+  pinMode(ball_sense_feedback_pin, INPUT);
+  pinMode(denso_sense_pin_2, INPUT);
+  pinMode(denso_sense_pin_3, INPUT);
+  pinMode(led_data_pin, INPUT);
+  pinMode(led2_data_pin, INPUT);
+  pinMode(ball_sense_pwm_pin, INPUT);
+
+  pinMode(shooting_servo_pin, OUTPUT);
+  pinMode(tamping_servo_pin, OUTPUT);
+  pinMode(relay_control_pin, OUTPUT);
+
+  pinMode(5, OUTPUT);
+  digitalWrite(5, LOW);
+
+  digitalWrite(relay_control_pin, LOW);
+
+  firing_servo.attach(shooting_servo_pin);
+  firing_servo.write(170);
+
+  tamping_servo.attach(tamping_servo_pin);
+  tamping_servo.write(180);
+}
+
+void loop() {
+  check_serial_input();
+  get_current_actual_pressure();
+  pressurize_if_needed();
+  tamp_if_needed();
+  shoot_if_needed();
+  print_state();
+  delay(50);
+}
+
+// Set Pressure|Tamp?|Shoot?|
+// XXX|X|X|
+void check_serial_input() {
+  if (Serial.available() > 0) {
+    while (Serial.available() > 0) {
+      read_string += (char)Serial.read();
+    }
+
+    if (read_string.endsWith("\n")) {
+      // Vairables to break things up
+      int last_substring_breakpoint = 0;
+      int current_substring_breakpoint = 0;
+
+      // Get substring and turn into pressure
+      current_substring_breakpoint = read_string.substring(last_substring_breakpoint).indexOf('|');
+      current_set_pressure = read_string.substring(last_substring_breakpoint, current_substring_breakpoint + last_substring_breakpoint).toInt();
+      last_substring_breakpoint += current_substring_breakpoint + 1;
+
+      // Get whether we should tamp
+      current_substring_breakpoint = read_string.substring(last_substring_breakpoint).indexOf('|');
+      should_tamp = read_string.substring(last_substring_breakpoint, current_substring_breakpoint + last_substring_breakpoint).toInt();
+      last_substring_breakpoint += current_substring_breakpoint + 1;
+
+
+      // Get whether we should shoot
+      current_substring_breakpoint = read_string.substring(last_substring_breakpoint).indexOf('|');
+      should_shoot = read_string.substring(last_substring_breakpoint, current_substring_breakpoint + last_substring_breakpoint).toInt();
+      last_substring_breakpoint += current_substring_breakpoint + 1;
+
+      read_string = "";
+    }
+
+  }
+}
+
+void get_current_actual_pressure() {
+  current_actual_pressure = read_pressure();
+}
+
+void pressurize_if_needed() {
+  static bool keep_charging = false;
+
+  if (current_set_pressure > 0) {
+
+    if (keep_charging) {
+      digitalWrite(relay_control_pin, HIGH);
+
+      if (current_actual_pressure >= current_set_pressure) {
+        keep_charging = false;
+        digitalWrite(relay_control_pin, LOW);
+      }
+    }
+
+    if ((current_set_pressure - current_actual_pressure) > allowed_psi_error) {
+      keep_charging = true;
+    }
+
+  } else {
+    digitalWrite(relay_control_pin, LOW);
+  }
+}
+
+void tamp_if_needed() {
+  if (should_tamp) {
+
+    tamping_servo.write(60);
+
+    delay(1000);
+    tamping_servo.write(180);
+
+    should_tamp = false;
+  }
+}
+
+void shoot_if_needed() {
+  if (should_shoot) {
+    firing_servo.write(100);
+    delay(500);
+    firing_servo.write(170);
+
+    current_set_pressure = 0;
+    should_shoot = false;
+  }
+}
+
+void print_state() {
+  Serial.print(current_actual_pressure);
+  Serial.print("|");
+  Serial.print(get_set_pressure());
+  Serial.print("|");
+  Serial.print(get_compressor_on());
+  Serial.print("|");
+  Serial.print(ball_detected());
+  Serial.print("|");
+  Serial.print(arm_motor_power_on());
+  Serial.println();
+}
+
+bool arm_motor_power_on() {
+  return digitalRead(denso_sense_pin_2);
+}
+
+bool ball_detected() {
+  return digitalRead(ball_sense_feedback_pin);
+}
+
+bool get_compressor_on() {
+  return digitalRead(relay_control_pin);
+}
+
+float get_set_pressure() {
+  return current_set_pressure;
+}
+
+float read_pressure() {
+  analogRead(pressure_sensor_pin);
+  uint16_t raw_adc = analogRead(pressure_sensor_pin);
+
+  float voltage = 0.004887585532746823 * (float) raw_adc;  //ADC count (10 bit) to voltage conversion
+
+  float pressure = (250000.0 * voltage - 250000.0) * 0.000145038; //Convert voltage to pascals, then to PSI
+
+  return max(pressure, 0);
+}

OSU Coursework/ROB 421 - Applied Robotics/software/firmware/led_controller/led_controller.ino

@@ -0,0 +1,74 @@
+#include "FastLED.h"
+#include <ArduinoJson.h>
+
+////////// Pinout Variables //////////
+const int led_power_status_pin = 8;
+const int led_general_status_pin = 9;
+const int power_on_status_pin = 7;
+const int led_pwm_pin = 6;
+
+////////// Global Variables //////////
+const int num_power_status_leds = 69;
+CRGB power_status_leds[num_power_status_leds];
+
+const int num_general_status_leds = 18;
+CRGB general_status_leds[num_general_status_leds];
+
+char read_buffer[1500];
+
+bool power_on = false;
+bool was_on = false;
+
+
+void setup() {
+  Serial.begin(57600);
+
+  FastLED.addLeds<WS2812, led_power_status_pin, GRB>(power_status_leds, num_power_status_leds).setCorrection(TypicalLEDStrip);
+  FastLED.addLeds<WS2812, led_general_status_pin, GRB>(general_status_leds, num_general_status_leds).setCorrection(TypicalLEDStrip);
+
+  FastLED.setBrightness(40);
+
+  pinMode(power_on_status_pin, INPUT);
+  pinMode(led_pwm_pin, OUTPUT);
+
+  analogWriteFrequency(led_pwm_pin, 38000);
+  analogWrite(led_pwm_pin, 128);
+}
+
+void loop() {
+  if (Serial.available() > 0) {
+    Serial.readBytesUntil('\n', read_buffer, 1500);
+  }
+
+  StaticJsonBuffer<1500> jsonBuffer;
+  JsonObject& root = jsonBuffer.parseObject(read_buffer);
+
+  if (root.success()) {
+    Serial.println("worked!");
+    JsonArray& leds = root["leds"];
+
+    int led_count = 0;
+    for (unsigned int i = 0 ; i < min(num_general_status_leds * 3, leds.size()) ; i += 3) {
+      general_status_leds[led_count].red = leds[i];
+      general_status_leds[led_count].green = leds[i + 1];
+      general_status_leds[led_count].blue = leds[i + 2];
+      led_count++;
+    }
+  }
+
+  EVERY_N_MILLISECONDS(250) {
+    if (digitalRead(power_on_status_pin)) {
+      if (was_on) {
+        fill_solid(power_status_leds, num_power_status_leds, CRGB::Black);
+        was_on = 0;
+      } else {
+        fill_solid(power_status_leds, num_power_status_leds, CRGB::Red);
+        was_on = 1;
+      }
+    } else {
+      fill_solid(power_status_leds, num_power_status_leds, CRGB::Green);
+    }
+  }
+
+  FastLED.show();
+}

OSU Coursework/ROB 421 - Applied Robotics/software/firmware/sensor_demo/sensor_demo.ino

@@ -0,0 +1,148 @@
+#define PRESSURE_INPUT_PIN A5
+#define VALVE_CONTROL_PIN 9
+#define COMPRESSOR_CONTROL_PIN 2
+
+#define LED_PIN 13
+
+//Code safeties
+#define SAFETY_PSI_MAX 25
+
+String input;
+
+void setup() {
+
+  pinMode(COMPRESSOR_CONTROL_PIN, OUTPUT);
+  digitalWrite(COMPRESSOR_CONTROL_PIN, LOW);
+  pinMode(LED_PIN, OUTPUT);
+  digitalWrite(LED_PIN, LOW);
+  pinMode(PRESSURE_INPUT_PIN, INPUT);
+
+  pinMode(VALVE_CONTROL_PIN, OUTPUT);
+  digitalWrite(VALVE_CONTROL_PIN, HIGH);
+
+  Serial.begin(9600);
+}
+
+void loop() {
+  uint16_t targetPSI = 0;
+  float    currentPSI = 0;
+  uint8_t  shouldCompress = 0;
+  digitalWrite(VALVE_CONTROL_PIN, HIGH) ;
+  
+  if(Serial.available()){
+    input = Serial.readStringUntil('\n');
+    Serial.flush();
+
+    if(isValidNumber(input)){
+      targetPSI = input.toInt();
+      
+      Serial.print("Target PSI: ");
+      Serial.println(targetPSI);
+
+      if(targetPSI > SAFETY_PSI_MAX){
+        Serial.print("Target PSI too high, bounding to safety max of ");
+        Serial.println(SAFETY_PSI_MAX);
+      }
+
+      targetPSI = constrain(targetPSI, 0, SAFETY_PSI_MAX);
+      
+      shouldCompress = 1;
+
+      //Start the compressor
+      digitalWrite(COMPRESSOR_CONTROL_PIN, HIGH);
+      digitalWrite(LED_PIN, HIGH);
+
+      while(shouldCompress){
+        currentPSI = readPressure();
+        if(currentPSI >= targetPSI){
+          shouldCompress = 0;
+        }
+        Serial.print("Current Pressure: ");
+        Serial.print(currentPSI);
+        Serial.print(" psi | Target Pressure: ");
+        Serial.print(targetPSI);
+        Serial.println(" psi");
+        delay(150);
+      }
+
+      //Stop the compressor
+      digitalWrite(COMPRESSOR_CONTROL_PIN, LOW);
+      digitalWrite(LED_PIN, LOW);
+
+      //Wait for half a second
+      delay(500) ;
+      //Release Valve
+      digitalWrite(VALVE_CONTROL_PIN, LOW) ;
+      //Wait to let all the pressure out
+      delay(2500) ;
+    }
+    
+  }
+
+  delay(150);  // ¯\_(ツ)_/¯
+
+  Serial.print("Current Pressure: ");
+  Serial.print(readPressure());
+  Serial.println(" psi");
+  Serial.flush();
+
+
+  /*
+  //Turn off the compressor
+  digitalWrite(COMPRESSOR_CONTROL_PIN, LOW);
+  digitalWrite(LED_PIN, LOW);
+  for(int i = 0; i < 2; ++i){
+    delay(160);
+    Serial.print("Read Analog Value: ");
+    Serial.print(readPressure());
+    Serial.println(" psi");
+    Serial.flush();
+  }
+
+  */
+
+  /*
+  //Turn on the compressor
+  digitalWrite(COMPRESSOR_CONTROL_PIN, HIGH);
+  digitalWrite(LED_PIN, HIGH);
+  for(int i = 0; i < 6; ++i){
+    delay(160);
+    Serial.print("Read Analog Value: ");
+    Serial.print(readPressure());
+    Serial.println(" psi");
+    Serial.flush();
+  }
+  */
+
+
+}
+
+/*
+ * 
+ * Pressure is a signal from 1 - 5 volts with 5 volts signifying 1 MPa 
+ * See http://nickmccomb.ddns.net:8090/display/AR/Sensors#Sensors-ISE50-T2-62L
+ */
+float readPressure(){
+  analogRead(PRESSURE_INPUT_PIN);
+  uint16_t raw_adc = analogRead(PRESSURE_INPUT_PIN);
+
+  float voltage = 0.004887585532746823 * (float) raw_adc;  //ADC count (10 bit) to voltage conversion
+
+  float pressure = (250000.0 * voltage -250000.0) * 0.000145038; //Convert voltage to pascals, then to PSI
+
+  return pressure;
+}
+
+
+boolean isValidNumber(String str){
+   for(byte i=0;i<str.length();i++)
+   {
+      if(isDigit(str.charAt(i))) return true;
+        }
+   return false;
+} 
+
+
+
+
+

OSU Coursework/ROB 421 - Applied Robotics/software/ground_station_setup.sh

@@ -0,0 +1,50 @@
+#!/usr/bin/env bash
+# As a point of reference, the environment layout should be as follows
+# /home/$user/Github/Rover_2017_2018 for the OSURC github repo
+# /home/$user/catkin_workspace for the user's catkin catkin_workspace
+# By keeping this consistent across all development machines, it will make it
+# easier to keep track of things
+
+# Which folders should be symbolically_linked?
+folders_to_link=(
+    denso_main
+    denso_master
+    denso_ground_station
+    denso_interface_controller
+)
+
+# Print heading
+echo "Setting up ROS packages for ground_station."
+
+# Get the catkin_workspace directory
+catkin_workspace_dir="applied_catkin"
+catkin_workspace_path="$HOME/$catkin_workspace_dir"
+catkin_src_path="$catkin_workspace_path/src"
+
+# Get the rover software directory
+github_rover_repo_dir="Github/team17_applied_robotics"
+github_rover_packages_path="$HOME/$github_rover_repo_dir/software/ros_packages"
+
+# Remove existing symbolic links if necessary
+symlinked_folders=$(find $catkin_src_path -maxdepth 1 -type l)
+if [ -z $symlinked_folders ]; then
+    echo "No symlinks to remove from catkin_workspace. Skipping."
+else
+    echo "Removing existing symlinks in catkin_workspace."
+    rm $symlinked_folders
+fi
+
+# Make the new symbolic link connections
+echo "Making new symlinks."
+for folder in ${folders_to_link[@]}; do
+    ln -s "$github_rover_packages_path/$folder" "$catkin_src_path/."
+    echo "Adding symlink for $folder."
+done
+
+# catkin_make so the new pacakges are available and re-source bash
+cd "$catkin_workspace_path"
+catkin_make
+
+source ~/.bashrc
+
+exit 0

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/CMakeLists.txt

@@ -0,0 +1,197 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(denso_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)

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>denso_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>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/scripts/ground_station_launch.sh

@@ -0,0 +1,20 @@
+#!/usr/bin/env bash
+
+export ROS_MASTER_URI=http://192.168.1.14:11311
+export ROS_IP=192.168.1.15
+
+current_folder_name="scripts"
+current_folder_name_length=`expr length $current_folder_name`
+
+launch_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+launch_dir_length=`expr length $launch_dir`
+
+launch_dir_length_without_current_folder=$(($launch_dir_length-$current_folder_name_length))
+
+script_launch_path="${launch_dir:0:$launch_dir_length_without_current_folder}/src"
+cd $script_launch_path
+
+sleep 1
+
+export DISPLAY=:0
+python ground_station.py

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Framework/ArmControlSystems/ArmControlSender.py

@@ -0,0 +1,103 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+from time import time
+
+import rospy
+from std_msgs.msg import Float32MultiArray
+
+#####################################
+# Global Variables
+#####################################
+DEFAULT_ARM_COMMAND_TOPIC = "/denso_control/relative_position"
+
+DRIVE_COMMAND_HERTZ = 40
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class ArmControlSender(QtCore.QThread):
+    def __init__(self, shared_objects):
+        super(ArmControlSender, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.spacenav_thread = self.shared_objects["threaded_classes"]["Spacenav Sender"]
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+        # Publishers
+        self.arm_command_publisher = rospy.Publisher(DEFAULT_ARM_COMMAND_TOPIC, Float32MultiArray, queue_size=1)
+
+        self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
+
+        self.spacenav_data = None
+        self.new_data = False
+
+    def run(self):
+        while self.run_thread_flag:
+
+            start_time = time()
+
+            self.send_arm_control_data()
+
+            time_diff = time() - start_time
+            self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
+
+    def send_arm_control_data(self):
+        if self.new_data:
+            message = Float32MultiArray()
+
+            multiplier = 3
+
+            message.data = (
+                self.spacenav_data["linear_y"] * multiplier,
+                -self.spacenav_data["linear_x"] * multiplier,
+                0,
+                0,
+                0,
+                0
+            )
+
+            # message.data = (
+            #     0,
+            #     0,
+            #     0,
+            #     0, #self.spacenav_data["angular_x"],
+            #     0, #self.spacenav_data["angular_y"],
+            #     self.spacenav_data["angular_z"]
+            # )
+
+            # message.data = (
+            #     self.spacenav_data["linear_y"],
+            #     self.spacenav_data["linear_x"],
+            #     self.spacenav_data["linear_z"],
+            #     self.spacenav_data["angular_x"],
+            #     self.spacenav_data["angular_y"],
+            #     self.spacenav_data["angular_z"]
+            # )
+            self.arm_command_publisher.publish(message)
+
+            # print self.spacenav_data
+            self.new_data = False
+
+    def on_spacenav_update_ready__slot(self, data):
+        self.spacenav_data = data
+        self.new_data = True
+        # print self.spacenav_data
+
+    def connect_signals_and_slots(self):
+        self.spacenav_thread.spacenav_state_update__signal.connect(self.on_spacenav_update_ready__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

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Framework/GameSystems/GameManagerCore.py

@@ -0,0 +1,507 @@
+#!/usr/bin/env python
+# coding=utf-8
+
+import rospy
+from PyQt5 import QtWidgets, QtCore, QtGui, uic
+
+import random
+import time
+
+import Resources.definitions as definitions
+from denso_master.msg import DensoStatusMessage
+from denso_interface_controller.msg import InterfaceStatusMessage, InterfaceControlMessage
+from std_msgs.msg import Float32MultiArray, UInt8MultiArray
+
+DENSO_STATUS_TOPIC_NAME = "/denso_status"
+DENSO_ABSOLUTE_JOINTS_TOPIC_NAME = "/denso_control/absolute_joints"
+
+DENSO_INTERFACE_CONTROLLER_CONTROL = "/denso_interface_controller/control"
+DENSO_INTERFACE_CONTROLLER_STATUS = "/denso_interface_controller/status"
+DENSO_LED_CONTROLLER_CONTROL = "/denso_led_controller/control"
+
+COLOR_BLUE = (0, 0, 255)
+COLOR_RED = (255, 0, 0)
+COLOR_YELLOW = (255, 255, 0)
+COLOR_PURPLE = (255, 0, 255)
+COLOR_ORANGE = (255, 165, 0)
+COLOR_BLACK = (0, 0, 0)
+
+AXES_DEGREES_ERROR = 0.01
+
+
+class GameManager(QtCore.QThread):
+    # ########## create signals for slots ##########
+    update_degrees_from_center_friendly__signal = QtCore.pyqtSignal(int)
+    update_degrees_from_45_friendly__signal = QtCore.pyqtSignal(int)
+    update_psi_friendly__signal = QtCore.pyqtSignal(int)
+
+    update_degrees_from_center_adversary__signal = QtCore.pyqtSignal(int)
+    update_degrees_from_45_adversary__signal = QtCore.pyqtSignal(int)
+    update_psi_adversary__signal = QtCore.pyqtSignal(int)
+
+    def __init__(self, shared_objects):
+        super(GameManager, self).__init__()
+
+        self.run_thread_flag = True
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.left_screen = self.shared_objects["screens"]["left_screen"]
+
+        # ########## set vars to gui elements
+        self.friendly_degrees_from_center_spinbox = self.left_screen.friendly_degrees_from_center_spinbox  # type: QtWidgets.QSpinBox
+        self.friendly_degrees_from_45_spinbox = self.left_screen.friendly_degrees_from_45_spinbox  # type: QtWidgets.QSpinBox
+        self.friendly_psi_spinbox = self.left_screen.friendly_psi_spinbox  # type: QtWidgets.QSpinBox
+        self.run_friendly_pushbutton = self.left_screen.run_friendly_pushbutton  # type: QtWidgets.QPushButton
+        self.friendly_continuous_run_checkbox = self.left_screen.friendly_continuous_run_checkbox  # type: QtWidgets.QCheckBox
+
+        self.adversary_degrees_from_center_spinbox = self.left_screen.adversary_degrees_from_center_spinbox  # type: QtWidgets.QSpinBox
+        self.adversary_degrees_from_45_spinbox = self.left_screen.adversary_degrees_from_45_spinbox  # type: QtWidgets.QSpinBox
+        self.adversary_psi_spinbox = self.left_screen.adversary_psi_spinbox  # type: QtWidgets.QSpinBox
+        self.run_adversary_pushbutton = self.left_screen.run_adversary_pushbutton  # type: QtWidgets.QPushButton
+        self.adversary_continuous_run_checkbox = self.left_screen.adversary_continuous_run_checkbox  # type: QtWidgets.QCheckBox
+
+        self.z1s1_pushbutton = self.left_screen.z1s1_pushbutton  # type: QtWidgets.QPushButton
+        self.z1s2_pushbutton = self.left_screen.z1s2_pushbutton  # type: QtWidgets.QPushButton
+        self.z2s3_pushbutton = self.left_screen.z2s3_pushbutton  # type: QtWidgets.QPushButton
+        self.z2s4_pushbutton = self.left_screen.z2s4_pushbutton  # type: QtWidgets.QPushButton
+        self.z2s5_pushbutton = self.left_screen.z2s5_pushbutton  # type: QtWidgets.QPushButton
+        self.z3s6_pushbutton = self.left_screen.z3s6_pushbutton  # type: QtWidgets.QPushButton
+        self.z3s7_pushbutton = self.left_screen.z3s7_pushbutton  # type: QtWidgets.QPushButton
+        self.z3s8_pushbutton = self.left_screen.z3s8_pushbutton  # type: QtWidgets.QPushButton
+        self.z3s9_pushbutton = self.left_screen.z3s9_pushbutton  # type: QtWidgets.QPushButton
+
+        self.global_offset_spinbox = self.left_screen.global_offset_spinbox  # type: QtWidgets.QSpinBox
+
+        self.BUTTON_MAPPINGS_FRIENDLY = {
+            self.z1s1_pushbutton: definitions.Z1_S1_FRIENDLY,
+            self.z1s2_pushbutton: definitions.Z1_S2_FRIENDLY,
+
+            self.z2s3_pushbutton: definitions.Z2_S3_FRIENDLY,
+            self.z2s4_pushbutton: definitions.Z2_S4_FRIENDLY,
+            self.z2s5_pushbutton: definitions.Z2_S5_FRIENDLY,
+
+            self.z3s6_pushbutton: definitions.Z3_S6_FRIENDLY,
+            self.z3s7_pushbutton: definitions.Z3_S7_FRIENDLY,
+            self.z3s8_pushbutton: definitions.Z3_S8_FRIENDLY,
+            self.z3s9_pushbutton: definitions.Z3_S9_FRIENDLY
+        }
+
+        self.BUTTON_MAPPINGS_ADVERSARY = {
+            self.z1s1_pushbutton: definitions.Z1_S1_ADVERSARY,
+            self.z1s2_pushbutton: definitions.Z1_S2_ADVERSARY,
+
+            self.z2s3_pushbutton: definitions.Z2_S3_ADVERSARY,
+            self.z2s4_pushbutton: definitions.Z2_S4_ADVERSARY,
+            self.z2s5_pushbutton: definitions.Z2_S5_ADVERSARY,
+
+            self.z3s6_pushbutton: definitions.Z3_S6_ADVERSARY,
+            self.z3s7_pushbutton: definitions.Z3_S7_ADVERSARY,
+            self.z3s8_pushbutton: definitions.Z3_S8_ADVERSARY,
+            self.z3s9_pushbutton: definitions.Z3_S9_ADVERSARY
+        }
+
+        self.status_subscriber = rospy.Subscriber(DENSO_STATUS_TOPIC_NAME, DensoStatusMessage,
+                                                  self.on_new_denso_status_update_received)
+
+        self.controller_status_subscriber = rospy.Subscriber(DENSO_INTERFACE_CONTROLLER_STATUS, InterfaceStatusMessage,
+                                                             self.on_new_controller_status_update_received)
+
+        self.abs_joints_publisher = rospy.Publisher(DENSO_ABSOLUTE_JOINTS_TOPIC_NAME, Float32MultiArray, queue_size=1)
+
+        self.led_controller_publisher = rospy.Publisher(DENSO_LED_CONTROLLER_CONTROL, UInt8MultiArray, queue_size=1)
+
+        self.interface_controller_publisher = rospy.Publisher(DENSO_INTERFACE_CONTROLLER_CONTROL, InterfaceControlMessage, queue_size=1)
+        self.interface_controller_message = InterfaceControlMessage()
+
+        self.run_friendly_flag = False
+        self.run_adversary_flag = False
+
+        self.denso_status = None  # type: DensoStatusMessage
+        self.interface_controller_status = None
+
+        self.last_color = COLOR_YELLOW
+
+        self.zone_pressed = False
+
+    def run(self):
+        while self.run_thread_flag:
+            self.show_color(COLOR_BLUE)
+
+            if self.run_friendly_flag:
+                self.run_friendly()
+            elif self.run_adversary_flag:
+                self.run_adversary()
+            self.msleep(20)
+
+    # ##### For J5, increasing values makese it point down #####
+    # ##### For J1, increasing values is counterclockwise and increasing values #####
+
+    def run_friendly(self):
+        # ##### Show Friendly Lights :) #####
+        self.show_color(COLOR_YELLOW)
+        self.last_color = COLOR_YELLOW
+
+        # ##### Wait for ball to be detected #####
+        while not self.interface_controller_status.ball_detected:
+            if self.last_color == COLOR_YELLOW:
+                self.show_color(COLOR_BLACK)
+                self.last_color = COLOR_BLACK
+            else:
+                self.show_color(COLOR_YELLOW)
+                self.last_color = COLOR_YELLOW
+            self.msleep(20)
+
+        # ##### Start at Fire Position #####
+        self.move_arm_to_position_and_wait(definitions.FIRE_JOINT_POSITIONS_MESSAGE)
+
+        # ##### Wait for zone click if continuous #####
+        if self.friendly_continuous_run_checkbox.isChecked():
+            while not self.zone_pressed:
+                if self.last_color == COLOR_PURPLE:
+                    self.show_color(COLOR_BLACK)
+                    self.last_color = COLOR_BLACK
+                else:
+                    self.show_color(COLOR_PURPLE)
+                    self.last_color = COLOR_PURPLE
+                self.msleep(20)
+
+        self.zone_pressed = False
+
+        # ##### Get values from gui #####
+        psi = self.friendly_psi_spinbox.value()
+        degrees_from_center = self.friendly_degrees_from_center_spinbox.value()
+        degrees_from_45 = self.friendly_degrees_from_45_spinbox.value()
+
+        # ##### Charge tank and Tamp Ball #####
+        self.interface_controller_message.set_pressure = psi
+        self.interface_controller_publisher.publish(self.interface_controller_message)
+        self.interface_controller_message.should_tamp = 0
+
+        # ##### Wait until feedback says tank is charging #####
+        if (psi - self.interface_controller_status.current_actual_pressure) > 5:
+            if psi > 5:
+                while not self.interface_controller_status.compressor_on:
+                    self.msleep(50)
+
+        # ##### Adjust to fire angles if needed #####
+        fire_joint_positions = list(definitions.FIRE_JOINT_POSITIONS)
+        fire_joint_positions[0] -= degrees_from_center - self.global_offset_spinbox.value()
+        fire_joint_positions[4] -= degrees_from_45
+
+        self.abs_joints_publisher.publish(Float32MultiArray(data=tuple(fire_joint_positions)))
+
+        # ##### Wait for compressor to finish building pressure #####
+        while self.interface_controller_status.compressor_on:
+            self.msleep(100)
+
+        # ##### Countdown before fire #####
+        message = UInt8MultiArray()
+
+        last_time = time.time()
+        num_seconds = 2
+        current_time = time.time() - last_time
+        while current_time < num_seconds:
+            values = []
+            for i in range(18):
+                if i > (float(num_seconds - current_time) / num_seconds) * 18:
+                    values.append(0)
+                    values.append(0)
+                    values.append(0)
+                else:
+                    values.append(0)
+                    values.append(255)
+                    values.append(0)
+
+            message.data = values
+
+            self.led_controller_publisher.publish(message)
+            self.msleep(100)
+            current_time = time.time() - last_time
+
+        self.show_color(COLOR_RED)
+
+        # ##### Fire! #####
+        self.interface_controller_message.should_fire = 1
+        self.interface_controller_message.set_pressure = 0
+        self.interface_controller_publisher.publish(self.interface_controller_message)
+        self.interface_controller_message.should_fire = 0
+
+        # ##### Check if in continuous mode and continue if necessary #####
+        self.msleep(500)
+        if self.friendly_continuous_run_checkbox.isChecked():
+            self.interface_controller_message.set_pressure = psi
+            self.interface_controller_publisher.publish(self.interface_controller_message)
+
+        # ##### Wait before moving #####
+        self.msleep(500)
+
+        # ##### Move back to Fire Position #####
+        if degrees_from_center != 0 or degrees_from_45 != 0:
+            self.move_arm_to_position_and_wait(definitions.FIRE_JOINT_POSITIONS_MESSAGE)
+
+        # ##### Move to Catch Position #####
+        self.move_arm_to_position_and_wait(definitions.CATCH_JOINT_POSITIONS_MESSAGE)
+
+        # ##### Cancel run flag if not in continuous mode
+        if not self.friendly_continuous_run_checkbox.isChecked():
+            self.run_friendly_flag = False
+
+    def run_adversary(self):
+        # ##### Show Friendly Lights :) #####
+        self.show_color(COLOR_YELLOW)
+        self.last_color = COLOR_YELLOW
+
+        # ##### Wait for ball to be detected #####
+        while not self.interface_controller_status.ball_detected:
+            if self.last_color == COLOR_YELLOW:
+                self.show_color(COLOR_BLACK)
+                self.last_color = COLOR_BLACK
+            else:
+                self.show_color(COLOR_YELLOW)
+                self.last_color = COLOR_YELLOW
+            self.msleep(20)
+        self.show_color(COLOR_YELLOW)
+
+        # ##### Start at Fire Position #####
+        self.move_arm_to_position_and_wait(definitions.FIRE_JOINT_POSITIONS_MESSAGE)
+
+        # ##### Wait for zone click if continuous #####
+        if self.adversary_continuous_run_checkbox.isChecked():
+            while not self.zone_pressed:
+                if self.last_color == COLOR_PURPLE:
+                    self.show_color(COLOR_BLACK)
+                    self.last_color = COLOR_BLACK
+                else:
+                    self.show_color(COLOR_PURPLE)
+                    self.last_color = COLOR_PURPLE
+                self.msleep(20)
+            self.show_color(COLOR_PURPLE)
+
+        self.zone_pressed = False
+
+        # ##### Get values from gui #####
+        psi = self.adversary_psi_spinbox.value()
+        degrees_from_center = self.adversary_degrees_from_center_spinbox.value()
+        degrees_from_45 = self.adversary_degrees_from_45_spinbox.value()
+
+        # ##### Charge tank and Tamp Ball #####
+        self.interface_controller_message.set_pressure = psi
+        self.interface_controller_publisher.publish(self.interface_controller_message)
+        self.interface_controller_message.should_tamp = 0
+
+        # ##### Wait until feedback says tank is charging #####
+        if (psi - self.interface_controller_status.current_actual_pressure) > 5:
+            if psi > 5:
+                while not self.interface_controller_status.compressor_on:
+                    self.msleep(50)
+
+        # ##### Adjust to fire angles if needed #####
+        fire_joint_positions = list(definitions.FIRE_JOINT_POSITIONS)
+        fire_joint_positions[0] -= degrees_from_center - self.global_offset_spinbox.value()
+        fire_joint_positions[4] = random.randint(definitions.ADVERSARY_RANDOM_J5_MIN, definitions.ADVERSARY_RANDOM_J5_MAX)
+
+        self.abs_joints_publisher.publish(Float32MultiArray(data=tuple(fire_joint_positions)))
+
+        # ##### Wait for compressor to finish building pressure #####
+        while self.interface_controller_status.compressor_on:
+            self.move_arm_random_and_wait()
+
+        # ##### Preparation for firing #####
+        self.interface_controller_message.should_fire = 1
+        self.interface_controller_message.set_pressure = 0
+
+        # ##### Countdown before fire #####
+        message = UInt8MultiArray()
+
+        last_time = time.time()
+        num_seconds = 3
+        fakeout_time = random.randint(500, num_seconds * 1000) / 1000.0
+        current_time = time.time() - last_time
+
+        has_fired = False
+
+        print fakeout_time
+        while current_time < num_seconds:
+            values = []
+            for i in range(18):
+                if i > (float(num_seconds - current_time) / num_seconds) * 18:
+                    values.append(0)
+                    values.append(0)
+                    values.append(0)
+                else:
+                    values.append(0)
+                    values.append(255)
+                    values.append(0)
+
+            message.data = values
+
+            self.led_controller_publisher.publish(message)
+
+            if (time.time() - last_time) > fakeout_time:
+                self.move_arm_to_position_and_wait(self.get_adversary_fire_message())
+                self.interface_controller_publisher.publish(self.interface_controller_message)
+                has_fired = True
+                break
+            else:
+                self.move_arm_random_and_wait()
+                self.msleep(random.randint(1, 500))
+            current_time = time.time() - last_time
+
+        if not has_fired:
+            self.move_arm_to_position_and_wait(self.get_adversary_fire_message())
+            self.interface_controller_publisher.publish(self.interface_controller_message)
+
+        self.show_color(COLOR_RED)
+
+        # ##### Disable firing #####
+        self.interface_controller_message.should_fire = 0
+
+        # ##### Check if in continuous mode and continue if necessary #####
+        self.msleep(500)
+        if self.adversary_continuous_run_checkbox.isChecked():
+            self.interface_controller_message.set_pressure = psi
+            self.interface_controller_publisher.publish(self.interface_controller_message)
+
+        # ##### Wait before moving #####
+        self.msleep(500)
+
+        # ##### Move back to Fire Position #####
+        if degrees_from_center != 0 or degrees_from_45 != 0:
+            self.move_arm_to_position_and_wait(definitions.FIRE_JOINT_POSITIONS_MESSAGE)
+
+        # ##### Move to Catch Position #####
+        self.move_arm_to_position_and_wait(definitions.CATCH_JOINT_POSITIONS_MESSAGE)
+
+        # ##### Cancel run flag if not in continuous mode
+        if not self.adversary_continuous_run_checkbox.isChecked():
+            self.run_adversary_flag = False
+
+    def arm_arrived_at_position(self, desired_position):
+        if not self.denso_status:
+            return False
+
+        joint_positions = self.denso_status.joints
+
+        within_error = True
+
+        for i in range(len(joint_positions)):
+            if not (abs(joint_positions[i] - desired_position[i]) < AXES_DEGREES_ERROR):
+                within_error = False
+
+        return within_error
+
+    def move_arm_random_and_wait(self):
+        message = Float32MultiArray()
+        rand_j1 = random.randint(definitions.ADVERSARY_RANDOM_J1_MIN, definitions.ADVERSARY_RANDOM_J1_MAX)
+        rand_j5 = random.randint(definitions.ADVERSARY_RANDOM_J5_MIN, definitions.ADVERSARY_RANDOM_J5_MAX)
+
+        fire_message = list(definitions.FIRE_JOINT_POSITIONS)
+        fire_message[0] = rand_j1
+        fire_message[4] = rand_j5
+
+        message.data = tuple(fire_message)
+
+        self.move_arm_to_position_and_wait(message)
+
+    def get_adversary_fire_message(self):
+        degrees_from_center = self.adversary_degrees_from_center_spinbox.value()
+        degrees_from_45 = self.adversary_degrees_from_45_spinbox.value()
+
+        fire_joint_positions = list(definitions.FIRE_JOINT_POSITIONS)
+
+        message = Float32MultiArray()
+
+        if degrees_from_center >= 0:
+            rand_j1 = 3 - self.global_offset_spinbox.value()
+        else:
+            rand_j1 = -3 - self.global_offset_spinbox.value()
+
+        print rand_j1
+
+        rand_j5 = fire_joint_positions[4] - degrees_from_45
+
+        fire_message = list(definitions.FIRE_JOINT_POSITIONS)
+        fire_message[0] = rand_j1
+        fire_message[4] = rand_j5
+
+        message.data = tuple(fire_message)
+
+        return message
+
+    def show_color(self, color):
+        message = UInt8MultiArray()
+        values = []
+        for _ in range(18):
+            values.append(color[0])
+            values.append(color[1])
+            values.append(color[2])
+        message.data = values
+
+        self.led_controller_publisher.publish(message)
+
+    def move_arm_to_position_and_wait(self, message):
+        self.abs_joints_publisher.publish(message)
+        while not self.arm_arrived_at_position(message.data):
+            self.msleep(10)
+
+    def on_zone_pushbutton_clicked__slot(self):
+        event_sender = self.sender()
+
+        new_zone_friendly = self.BUTTON_MAPPINGS_FRIENDLY[event_sender]
+        new_zone_adversary = self.BUTTON_MAPPINGS_ADVERSARY[event_sender]
+
+        self.update_degrees_from_center_friendly__signal.emit(new_zone_friendly["degrees_from_center"])
+        self.update_degrees_from_45_friendly__signal.emit(new_zone_friendly["degrees_from_45"])
+        self.update_psi_friendly__signal.emit(new_zone_friendly["psi"])
+
+        self.update_degrees_from_center_adversary__signal.emit(new_zone_adversary["degrees_from_center"])
+        self.update_degrees_from_45_adversary__signal.emit(new_zone_adversary["degrees_from_45"])
+        self.update_psi_adversary__signal.emit(new_zone_adversary["psi"])
+
+        self.zone_pressed = True
+
+    def on_new_denso_status_update_received(self, data):
+        self.denso_status = data
+
+    def on_new_controller_status_update_received(self, data):
+        self.interface_controller_status = data
+
+    def on_friendly_clicked__slot(self):
+        if not self.run_adversary_flag:
+            self.run_friendly_flag = True
+
+    def on_adversary_clicked__slot(self):
+        if not self.run_friendly_flag:
+            self.run_adversary_flag = True
+
+    def connect_signals_and_slots(self):
+        self.run_friendly_pushbutton.clicked.connect(self.on_friendly_clicked__slot)
+        self.run_adversary_pushbutton.clicked.connect(self.on_adversary_clicked__slot)
+
+        self.z1s1_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+        self.z1s2_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+        self.z2s3_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+        self.z2s4_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+        self.z2s5_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+        self.z3s6_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+        self.z3s7_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+        self.z3s8_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+        self.z3s9_pushbutton.clicked.connect(self.on_zone_pushbutton_clicked__slot)
+
+        self.update_degrees_from_center_friendly__signal.connect(self.friendly_degrees_from_center_spinbox.setValue)
+        self.update_degrees_from_center_adversary__signal.connect(self.adversary_degrees_from_center_spinbox.setValue)
+
+        self.update_degrees_from_45_friendly__signal.connect(self.friendly_degrees_from_45_spinbox.setValue)
+        self.update_degrees_from_45_adversary__signal.connect(self.adversary_degrees_from_45_spinbox.setValue)
+
+        self.update_psi_friendly__signal.connect(self.friendly_psi_spinbox.setValue)
+        self.update_psi_adversary__signal.connect(self.adversary_psi_spinbox.setValue)
+
+    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

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Framework/InputSystems/JoystickControlSender.py

@@ -0,0 +1,234 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import logging
+from inputs import devices, GamePad
+from time import time
+
+import rospy
+from std_msgs.msg import Float32MultiArray
+
+#####################################
+# Global Variables
+#####################################
+GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro"
+
+DEFAULT_ARM_COMMAND_TOPIC = "/denso_control/relative_joints"
+
+DRIVE_COMMAND_HERTZ = 15
+
+Z_AXIS_DEADBAND = 0.2
+Y_AXIS_DEADBAND = 0.05
+X_AXIS_DEADBAND = 0.05
+
+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
+
+
+#####################################
+# 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 = {
+            "x_axis": 512,
+            "y_axis": 512,
+            "z_axis": 128,
+            "throttle_axis": 128,
+
+            "hat_x_axis": 0,
+            "hat_y_axis": 0,
+
+            "trigger_pressed": 0,
+            "thumb_pressed": 0,
+            "three_pressed": 0,
+            "four_pressed": 0,
+            "five_pressed": 0,
+            "six_pressed": 0,
+
+            "seven_pressed": 0,
+            "eight_pressed": 0,
+            "nine_pressed": 0,
+            "ten_pressed": 0,
+            "eleven_pressed": 0,
+            "twelve_pressed": 0,
+        }
+
+        self.raw_mapping_to_class_mapping = {
+            "ABS_X": "x_axis",
+            "ABS_Y": "y_axis",
+            "ABS_RZ": "z_axis",
+            "ABS_THROTTLE": "throttle_axis",
+
+            "ABS_HAT0X": "hat_x_axis",
+            "ABS_HAT0Y": "hat_y_axis",
+
+            "BTN_TRIGGER": "trigger_pressed",
+            "BTN_THUMB": "thumb_pressed",
+            "BTN_THUMB2": "three_pressed",
+            "BTN_TOP": "four_pressed",
+            "BTN_TOP2": "five_pressed",
+            "BTN_PINKIE": "six_pressed",
+
+            "BTN_BASE": "seven_pressed",
+            "BTN_BASE2": "eight_pressed",
+            "BTN_BASE3": "nine_pressed",
+            "BTN_BASE4": "ten_pressed",
+            "BTN_BASE5": "eleven_pressed",
+            "BTN_BASE6": "twelve_pressed",
+        }
+
+        self.ready = False
+
+        self.start()
+
+    def run(self):
+
+        while self.run_thread_flag:
+            if self.setup_controller_flag:
+                self.controller_acquired = self.__setup_controller()
+                self.setup_controller_flag = False
+            if self.data_acquisition_and_broadcast_flag:
+                self.__get_controller_data()
+
+    def __setup_controller(self):
+        for device in devices.gamepads:
+            if device.name == GAME_CONTROLLER_NAME:
+                self.gamepad = device
+
+                return True
+        return False
+
+    def __get_controller_data(self):
+        if self.controller_acquired:
+            events = self.gamepad.read()
+
+            for event in events:
+                if event.code in self.raw_mapping_to_class_mapping:
+                    self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
+                    # print event.code
+
+            self.ready = True
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class JoystickControlSender(QtCore.QThread):
+    def __init__(self, shared_objects):
+        super(JoystickControlSender, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+
+        # ########## 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.arm_command_publisher = rospy.Publisher(DEFAULT_ARM_COMMAND_TOPIC, Float32MultiArray, queue_size=1)
+
+        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()
+
+    def run(self):
+        while self.run_thread_flag:
+
+            start_time = time()
+
+            self.check_and_set_pause_state()
+            self.__update_and_publish()
+
+            time_diff = time() - start_time
+            self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
+
+    def connect_signals_and_slots(self):
+        pass
+
+    def check_and_set_pause_state(self):
+        thumb_pressed = self.joystick.controller_states["thumb_pressed"]
+        if thumb_pressed and (time() - self.last_pause_state_time) > PAUSE_STATE_CHANGE_TIME:
+            self.drive_paused = not self.drive_paused
+            self.last_pause_state_time = time()
+
+    def __update_and_publish(self):
+        self.publish_drive_command()
+
+    def publish_drive_command(self):
+        if self.drive_paused:
+            pass
+
+        else:
+            arm_message = self.get_control_message()
+            self.arm_command_publisher.publish(arm_message)
+
+        # print arm_message
+        # self.arm_command_publisher.publish(arm_message)
+
+    def get_control_message(self):
+        control_message = Float32MultiArray()
+
+        multiplier = 1.75
+
+        j1 = self.joystick.controller_states["eleven_pressed"]
+        j2 = self.joystick.controller_states["twelve_pressed"]
+        j3 = self.joystick.controller_states["nine_pressed"]
+        j4 = self.joystick.controller_states["ten_pressed"]
+        j5 = self.joystick.controller_states["seven_pressed"]
+        j6 = self.joystick.controller_states["eight_pressed"]
+
+        y_axis = multiplier * (-(self.joystick.controller_states["y_axis"] - 512) / 512.0)
+
+        if abs(y_axis) < Y_AXIS_DEADBAND:
+            y_axis = 0
+
+        control_message.data = (
+            y_axis if j1 else 0,
+            y_axis if j2 else 0,
+            y_axis if j3 else 0,
+            y_axis if j4 else 0,
+            y_axis if j5 else 0,
+            y_axis if j6 else 0,
+        )
+
+        return control_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

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Framework/InputSystems/SpaceNavControlSender.py

@@ -0,0 +1,155 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore
+import logging
+from time import time
+import spnav
+
+import rospy
+from std_msgs.msg import Float32MultiArray
+
+#####################################
+# Global Variables
+#####################################
+THREAD_HERTZ = 15
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class SpaceNavControlSender(QtCore.QThread):
+    spacenav_state_update__signal = QtCore.pyqtSignal(object)
+
+    CONTROL_MODE = 0
+    WAIT_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"
+        }
+
+        self.current_control_mode = self.WAIT_MODE
+
+    def run(self):
+        spnav.spnav_open()
+
+        while self.run_thread_flag:
+
+            start_time = time()
+
+            self.process_spnav_events()
+            self.check_control_mode_change()
+            self.send_arm_control_commands()
+
+            time_diff = time() - start_time
+
+            self.msleep(max(int(self.wait_time - time_diff * 1000), 0))
+
+    def process_spnav_events(self):
+        event = spnav.spnav_poll_event()
+
+        if 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.CONTROL_MODE
+        elif self.spnav_states["2_pressed"]:
+            self.current_control_mode = self.WAIT_MODE
+
+    def send_arm_control_commands(self):
+        if self.current_control_mode == self.CONTROL_MODE:
+            self.spacenav_state_update__signal.emit(self.spnav_states)
+        elif self.current_control_mode == self.WAIT_MODE:
+            pass
+
+    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

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Framework/StartupSystems/ROSMasterChecker.py

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

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Framework/StatusSystems/StatusCore.py

@@ -0,0 +1,316 @@
+#!/usr/bin/env python
+# coding=utf-8
+
+import rospy
+from PyQt5 import QtWidgets, QtCore, QtTest
+import paramiko
+import random
+import Resources.definitions as definitions
+from denso_master.msg import DensoStatusMessage
+from denso_interface_controller.msg import InterfaceStatusMessage, InterfaceControlMessage
+from std_msgs.msg import UInt8, Bool, Float32MultiArray, UInt8MultiArray
+
+
+DENSO_STATUS_TOPIC_NAME = "/denso_status"
+DENSO_SPEED_TOPIC_NAME = "/denso_control/speed"
+DENSO_MOTOR_TOPIC_NAME = "/denso_control/motors_enabled"
+
+DENSO_ABSOLUTE_JOINTS_TOPIC_NAME = "/denso_control/absolute_joints"
+
+DENSO_INTERFACE_CONTROLLER_STATUS = "/denso_interface_controller/status"
+DENSO_INTERFACE_CONTROLLER_CONTROL = "/denso_interface_controller/control"
+DENSO_LED_CONTROLLER_CONTROL = "/denso_led_controller/control"
+
+ACCESS_POINT_IP = "192.168.1.14"
+ACCESS_POINT_USER = "denso"
+ACCESS_POINT_PASSWORD = "denso"
+
+
+class SensorCore(QtCore.QThread):
+    # ########## create signals for slots ##########
+    motor_enabled_stylesheet_change__signal = QtCore.pyqtSignal(str)
+    arm_normal_stylesheet_change__signal = QtCore.pyqtSignal(str)
+    arm_busy_stylesheet_change__signal = QtCore.pyqtSignal(str)
+    error_stylesheet_change__signal = QtCore.pyqtSignal(str)
+    tank_charging_stylesheet_change__signal = QtCore.pyqtSignal(str)
+    ball_detected_stylesheet_change__signal = QtCore.pyqtSignal(str)
+
+    arm_speed_change__signal = QtCore.pyqtSignal(str)
+    tank_psi_change__signal = QtCore.pyqtSignal(str)
+    tank_psi_set_change__signal = QtCore.pyqtSignal(str)
+
+    x_changed__signal = QtCore.pyqtSignal(float)
+    y_changed__signal = QtCore.pyqtSignal(float)
+    z_changed__signal = QtCore.pyqtSignal(float)
+    rx_changed__signal = QtCore.pyqtSignal(float)
+    ry_changed__signal = QtCore.pyqtSignal(float)
+    rz_changed__signal = QtCore.pyqtSignal(float)
+
+    j1_changed__signal = QtCore.pyqtSignal(float)
+    j2_changed__signal = QtCore.pyqtSignal(float)
+    j3_changed__signal = QtCore.pyqtSignal(float)
+    j4_changed__signal = QtCore.pyqtSignal(float)
+    j5_changed__signal = QtCore.pyqtSignal(float)
+    j6_changed__signal = QtCore.pyqtSignal(float)
+
+    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.left_screen = self.shared_objects["screens"]["left_screen"]
+
+        # ########## set vars to gui elements
+        self.motor_enabled_label = self.left_screen.motor_enabled_label  # type: QtWidgets.QLabel
+        self.arm_speed_label = self.left_screen.arm_speed_label  # type: QtWidgets.QLabel
+        self.arm_normal_label = self.left_screen.arm_normal_label  # type: QtWidgets.QLabel
+        self.arm_busy_label = self.left_screen.arm_busy_label  # type: QtWidgets.QLabel
+        self.arm_error_label = self.left_screen.error_label  # type: QtWidgets.QLabel
+        self.tank_psi_label = self.left_screen.tank_psi_label  # type: QtWidgets.QLabel
+        self.set_psi_label = self.left_screen.set_psi_label  # type: QtWidgets.QLabel
+        self.compressor_on_label = self.left_screen.compressor_on_label  # type: QtWidgets.QLabel
+        self.ball_detected_label = self.left_screen.ball_detected_label  # type: QtWidgets.QLabel
+
+        self.motor_enable_button = self.left_screen.motor_enable_button  # type: QtWidgets.QPushButton
+        self.motor_disable_button = self.left_screen.motor_disable_button  # type: QtWidgets.QPushButton
+
+        self.arm_set_speed_spinbox = self.left_screen.arm_speed_spinbox  # type: QtWidgets.QSpinBox
+        self.arm_set_speed_button = self.left_screen.arm_set_speed_button  # type: QtWidgets.QPushButton
+
+        self.preset_catch_button = self.left_screen.preset_catch_button  # type: QtWidgets.QPushButton
+        self.preset_fire_button = self.left_screen.preset_fire_button  # type: QtWidgets.QPushButton
+
+        self.adversary_demo_button = self.left_screen.adversary_demo_button  # type: QtWidgets.QPushButton
+
+        self.set_psi_spinbox = self.left_screen.set_psi_spinbox  # type: QtWidgets.QSpinBox
+        self.set_psi_pushbutton = self.left_screen.set_psi_pushbutton  # type: QtWidgets.QPushButton
+
+        self.fire_button = self.left_screen.fire_button  # type: QtWidgets.QPushButton
+        self.tamp_button = self.left_screen.tamp_button  # type: QtWidgets.QPushButton
+
+        self.shutdown_systems_pushbutton = self.left_screen.shutdown_systems_pushbutton  # type: QtWidgets.QPushButton
+
+        self.x_lcdnumber = self.left_screen.x_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.y_lcdnumber = self.left_screen.y_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.z_lcdnumber = self.left_screen.z_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.rx_lcdnumber = self.left_screen.rx_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.ry_lcdnumber = self.left_screen.ry_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.rz_lcdnumber = self.left_screen.rz_lcdnumber  # type: QtWidgets.QLCDNumber
+
+        self.j1_lcdnumber = self.left_screen.j1_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.j2_lcdnumber = self.left_screen.j2_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.j3_lcdnumber = self.left_screen.j3_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.j4_lcdnumber = self.left_screen.j4_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.j5_lcdnumber = self.left_screen.j5_lcdnumber  # type: QtWidgets.QLCDNumber
+        self.j6_lcdnumber = self.left_screen.j6_lcdnumber  # type: QtWidgets.QLCDNumber
+
+        self.status_subscriber = rospy.Subscriber(DENSO_STATUS_TOPIC_NAME, DensoStatusMessage, self.on_new_status_update_received)
+        self.controller_status_subscriber = rospy.Subscriber(DENSO_INTERFACE_CONTROLLER_STATUS, InterfaceStatusMessage, self.on_new_controller_status_update_received)
+
+        self.abs_joints_publisher = rospy.Publisher(DENSO_ABSOLUTE_JOINTS_TOPIC_NAME, Float32MultiArray, queue_size=1)
+
+        self.speed_publisher = rospy.Publisher(DENSO_SPEED_TOPIC_NAME, UInt8, queue_size=1)
+        self.motor_enable_publisher = rospy.Publisher(DENSO_MOTOR_TOPIC_NAME, Bool, queue_size=1)
+
+        self.interface_controller_publisher = rospy.Publisher(DENSO_INTERFACE_CONTROLLER_CONTROL, InterfaceControlMessage, queue_size=1)
+        self.interface_controller_message = InterfaceControlMessage()
+
+        self.status_data = None
+        self.new_statuses = False
+
+        self.controller_status_data = None
+        self.new_controller_status_data = False
+
+        self.ssh_client = None
+
+    def run(self):
+        self.setup_and_connect_ssh_client()
+
+        while self.run_thread_flag:
+            self.update_statuses_if_needed()
+            self.update_controller_statuses_if_needed()
+            self.msleep(20)
+
+    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 on_new_status_update_received(self, data):
+        self.status_data = data
+        self.new_statuses = True
+
+    def on_new_controller_status_update_received(self, data):
+        self.controller_status_data = data
+        self.new_controller_status_data = True
+
+    def update_statuses_if_needed(self):
+        if self.new_statuses:
+            if self.status_data.motor_enabled:
+                self.motor_enabled_stylesheet_change__signal.emit("background-color:darkgreen;")
+            else:
+                self.motor_enabled_stylesheet_change__signal.emit("background-color:darkred;")
+
+            self.arm_speed_change__signal.emit(str(self.status_data.speed))
+
+            if self.status_data.arm_normal:
+                self.arm_normal_stylesheet_change__signal.emit("background-color:darkgreen;")
+            else:
+                self.arm_normal_stylesheet_change__signal.emit("background-color:darkred;")
+
+            if not self.status_data.arm_busy:
+                self.arm_busy_stylesheet_change__signal.emit("background-color:darkgreen;")
+            else:
+                self.arm_busy_stylesheet_change__signal.emit("background-color:darkred;")
+
+            if self.status_data.error == "":
+                self.error_stylesheet_change__signal.emit("background-color:darkgreen;")
+            else:
+                self.error_stylesheet_change__signal.emit("background-color:darkred;")
+
+            self.x_changed__signal.emit(self.status_data.positions[0])
+            self.y_changed__signal.emit(self.status_data.positions[1])
+            self.z_changed__signal.emit(self.status_data.positions[2])
+            self.rx_changed__signal.emit(self.status_data.positions[3])
+            self.ry_changed__signal.emit(self.status_data.positions[4])
+            self.rz_changed__signal.emit(self.status_data.positions[5])
+
+            self.j1_changed__signal.emit(self.status_data.joints[0])
+            self.j2_changed__signal.emit(self.status_data.joints[1])
+            self.j3_changed__signal.emit(self.status_data.joints[2])
+            self.j4_changed__signal.emit(self.status_data.joints[3])
+            self.j5_changed__signal.emit(self.status_data.joints[4])
+            self.j6_changed__signal.emit(self.status_data.joints[5])
+
+            self.new_statuses = False
+
+    def update_controller_statuses_if_needed(self):
+        if self.new_controller_status_data:
+            self.tank_psi_change__signal.emit(str(int(self.controller_status_data.current_actual_pressure)))
+            self.tank_psi_set_change__signal.emit(str(int(self.controller_status_data.current_set_pressure)))
+
+            if self.controller_status_data.compressor_on:
+                self.tank_charging_stylesheet_change__signal.emit("background-color:darkgreen;")
+            else:
+                self.tank_charging_stylesheet_change__signal.emit("background-color:darkred;")
+
+            if self.controller_status_data.ball_detected:
+                self.ball_detected_stylesheet_change__signal.emit("background-color:darkgreen;")
+            else:
+                self.ball_detected_stylesheet_change__signal.emit("background-color:darkred;")
+
+            self.new_controller_status_data = False
+
+    def on_tank_fire_pressed__slot(self):
+        self.interface_controller_message.should_fire = 1
+        self.interface_controller_message.set_pressure = 0
+        self.interface_controller_publisher.publish(self.interface_controller_message)
+        self.interface_controller_message.should_fire = 0
+
+    def on_tamp_pressed__slot(self):
+        self.interface_controller_message.should_tamp = 1
+        self.interface_controller_publisher.publish(self.interface_controller_message)
+        self.interface_controller_message.should_tamp = 0
+
+    def on_set_tank_psi_pressed__slot(self):
+        self.interface_controller_message.set_pressure = self.set_psi_spinbox.value()
+        self.interface_controller_publisher.publish(self.interface_controller_message)
+
+    def on_motor_enabled_pressed__slot(self):
+        self.motor_enable_publisher.publish(1)
+
+    def on_motor_disabled_pressed__slot(self):
+        self.motor_enable_publisher.publish(0)
+
+    def on_set_arm_speed_pressed__slot(self):
+        self.speed_publisher.publish(self.arm_set_speed_spinbox.value())
+
+    def on_catch_pressed__slot(self):
+        self.abs_joints_publisher.publish(definitions.CATCH_JOINT_POSITIONS_MESSAGE)
+
+    def on_fire_pressed__slot(self):
+        self.abs_joints_publisher.publish(definitions.FIRE_JOINT_POSITIONS_MESSAGE)
+
+    def on_adversary_demo_button_pressed__slot(self):
+
+        num_fakeouts = random.randint(8, 15)
+
+        for i in range(num_fakeouts):
+            message = Float32MultiArray()
+
+            rand_j1 = random.randint(definitions.ADVERSARY_RANDOM_J1_MIN, definitions.ADVERSARY_RANDOM_J1_MAX)
+            rand_j5 = random.randint(definitions.ADVERSARY_RANDOM_J5_MIN, definitions.ADVERSARY_RANDOM_J5_MAX)
+
+            fire_message = list(definitions.FIRE_JOINT_POSITIONS)
+            fire_message[0] = rand_j1
+            fire_message[4] = rand_j5
+
+            message.data = tuple(fire_message)
+
+            self.abs_joints_publisher.publish(message)
+
+        self.on_catch_pressed__slot()
+
+    def on_shutdown_systems_clicked__slot(self):
+        self.ssh_client.exec_command("/home/denso/Github/team17_applied_robotics/software/environment/denso_shutdown.sh")
+        self.msleep(1000)
+        QtTest.QTest.keyPress(self.left_screen, QtCore.Qt.Key_Q, QtCore.Qt.ControlModifier)
+        QtTest.QTest.keyRelease(self.left_screen, QtCore.Qt.Key_Q, QtCore.Qt.ControlModifier)
+
+    def connect_signals_and_slots(self):
+        self.motor_enabled_stylesheet_change__signal.connect(self.motor_enabled_label.setStyleSheet)
+        self.arm_normal_stylesheet_change__signal.connect(self.arm_normal_label.setStyleSheet)
+        self.arm_busy_stylesheet_change__signal.connect(self.arm_busy_label.setStyleSheet)
+        self.error_stylesheet_change__signal.connect(self.arm_error_label.setStyleSheet)
+        self.tank_charging_stylesheet_change__signal.connect(self.compressor_on_label.setStyleSheet)
+        self.ball_detected_stylesheet_change__signal.connect(self.ball_detected_label.setStyleSheet)
+
+        self.arm_speed_change__signal.connect(self.arm_speed_label.setText)
+        self.tank_psi_change__signal.connect(self.tank_psi_label.setText)
+        self.tank_psi_set_change__signal.connect(self.set_psi_label.setText)
+
+        self.x_changed__signal.connect(self.x_lcdnumber.display)
+        self.y_changed__signal.connect(self.y_lcdnumber.display)
+        self.z_changed__signal.connect(self.z_lcdnumber.display)
+        self.rx_changed__signal.connect(self.rx_lcdnumber.display)
+        self.ry_changed__signal.connect(self.ry_lcdnumber.display)
+        self.rz_changed__signal.connect(self.rz_lcdnumber.display)
+
+        self.j1_changed__signal.connect(self.j1_lcdnumber.display)
+        self.j2_changed__signal.connect(self.j2_lcdnumber.display)
+        self.j3_changed__signal.connect(self.j3_lcdnumber.display)
+        self.j4_changed__signal.connect(self.j4_lcdnumber.display)
+        self.j5_changed__signal.connect(self.j5_lcdnumber.display)
+        self.j6_changed__signal.connect(self.j6_lcdnumber.display)
+
+        self.motor_enable_button.clicked.connect(self.on_motor_enabled_pressed__slot)
+        self.motor_disable_button.clicked.connect(self.on_motor_disabled_pressed__slot)
+        self.arm_set_speed_button.clicked.connect(self.on_set_arm_speed_pressed__slot)
+
+        self.preset_catch_button.clicked.connect(self.on_catch_pressed__slot)
+        self.preset_fire_button.clicked.connect(self.on_fire_pressed__slot)
+
+        self.adversary_demo_button.clicked.connect(self.on_adversary_demo_button_pressed__slot)
+
+        self.set_psi_pushbutton.clicked.connect(self.on_set_tank_psi_pressed__slot)
+
+        self.fire_button.clicked.connect(self.on_tank_fire_pressed__slot)
+        self.tamp_button.clicked.connect(self.on_tamp_pressed__slot)
+
+        self.shutdown_systems_pushbutton.clicked.connect(self.on_shutdown_systems_clicked__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
+
+
+if __name__ == "__main__":
+    rover_statuses = SensorCore()
+    rover_statuses.run()

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py

@@ -0,0 +1,100 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets, QtGui
+import logging
+from time import time
+import cv2
+import qimage2ndarray
+import rospy
+from cv_bridge import CvBridge
+from sensor_msgs.msg import CompressedImage
+
+#####################################
+# Global Variables
+#####################################
+CAMERA_TOPIC_PATH = "/cameras/"
+EXCLUDED_CAMERAS = ["zed"]
+
+PRIMARY_LABEL_MAX = (640, 360)
+SECONDARY_LABEL_MAX = (640, 360)
+TERTIARY_LABEL_MAX = (640, 360)
+
+GUI_SELECTION_CHANGE_TIMEOUT = 3  # Seconds
+
+STYLESHEET_SELECTED = "border: 2px solid orange; background-color:black;"
+STYLESHEET_UNSELECTED = "background-color:black;"
+
+
+#####################################
+# RoverVideoCoordinator Class Definition
+#####################################
+class RoverVideoCoordinator(QtCore.QThread):
+    pixmap_ready_signal = QtCore.pyqtSignal()
+
+    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.video_label = self.right_screen.video_label  # type:QtWidgets.QLabel
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        self.camera_subscriber = rospy.Subscriber("/zed/right/image_raw_color/compressed", CompressedImage, self.__image_data_received_callback)
+
+        self.bridge = CvBridge()
+        self.raw_image = None
+        self.pixmap = None
+        self.new_frame = False
+
+    def run(self):
+        while self.run_thread_flag:
+            self.__show_video_enabled()
+            self.msleep(10)
+
+    def __show_video_enabled(self):
+        if self.new_frame:
+            try:
+                opencv_image = self.bridge.compressed_imgmsg_to_cv2(self.raw_image, "rgb8")
+
+                self.__create_final_pixmaps(opencv_image)
+            except Exception, error:
+                print "Failed with error:" + str(error)
+
+            self.new_frame = False
+
+    def __image_data_received_callback(self, raw_image):
+        self.raw_image = raw_image
+        self.new_frame = True
+
+    def __create_final_pixmaps(self, opencv_image):
+        height, width, _ = opencv_image.shape
+
+        opencv_image_sized = cv2.resize(opencv_image, (1920, 1080))
+
+        self.pixmap = QtGui.QPixmap.fromImage(qimage2ndarray.array2qimage(opencv_image_sized))
+        self.pixmap_ready_signal.emit()
+
+    def __on_pixmap_ready__slot(self):
+        try:
+            self.video_label.setPixmap(self.pixmap)
+        except:
+            print "text"
+
+    def connect_signals_and_slots(self):
+        self.pixmap_ready_signal.connect(self.__on_pixmap_ready__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

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Readme.md

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

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Resources/Ui/right_screen.ui

@@ -0,0 +1,72 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>1920</width>
+    <height>1080</height>
+   </rect>
+  </property>
+  <property name="sizePolicy">
+   <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+    <horstretch>0</horstretch>
+    <verstretch>0</verstretch>
+   </sizepolicy>
+  </property>
+  <property name="minimumSize">
+   <size>
+    <width>1920</width>
+    <height>1080</height>
+   </size>
+  </property>
+  <property name="maximumSize">
+   <size>
+    <width>1920</width>
+    <height>1080</height>
+   </size>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <property name="windowOpacity">
+   <double>1.000000000000000</double>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <layout class="QHBoxLayout" name="horizontalLayout">
+    <property name="spacing">
+     <number>0</number>
+    </property>
+    <property name="sizeConstraint">
+     <enum>QLayout::SetDefaultConstraint</enum>
+    </property>
+    <property name="leftMargin">
+     <number>0</number>
+    </property>
+    <property name="topMargin">
+     <number>0</number>
+    </property>
+    <property name="rightMargin">
+     <number>0</number>
+    </property>
+    <property name="bottomMargin">
+     <number>0</number>
+    </property>
+    <item>
+     <widget class="QLabel" name="video_label">
+      <property name="styleSheet">
+       <string notr="true">background-color:black;</string>
+      </property>
+      <property name="text">
+       <string/>
+      </property>
+     </widget>
+    </item>
+   </layout>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/Resources/definitions/__init__.py

@@ -0,0 +1,37 @@
+from std_msgs.msg import Float32MultiArray
+
+FIRE_JOINT_POSITIONS = (0, -53.5, 146.5, 0, 41.2, -8.73171)
+CATCH_JOINT_POSITIONS = (0, -95, 147, 0, 37.14258, -8.73171)
+
+CATCH_JOINT_POSITIONS_MESSAGE = Float32MultiArray(data=CATCH_JOINT_POSITIONS)
+FIRE_JOINT_POSITIONS_MESSAGE = Float32MultiArray(data=FIRE_JOINT_POSITIONS)
+
+ADVERSARY_RANDOM_J1_MIN = -27
+ADVERSARY_RANDOM_J1_MAX = 27
+
+ADVERSARY_RANDOM_J5_MIN = 25
+ADVERSARY_RANDOM_J5_MAX = 65
+
+Z1_S1_FRIENDLY = {"degrees_from_center": -10, "degrees_from_45": 0, "psi": 22}
+Z1_S2_FRIENDLY = {"degrees_from_center": 8, "degrees_from_45": 0, "psi": 22}
+
+Z2_S3_FRIENDLY = {"degrees_from_center": -14, "degrees_from_45": -5, "psi": 28}
+Z2_S4_FRIENDLY = {"degrees_from_center": 0, "degrees_from_45": -7, "psi": 28}
+Z2_S5_FRIENDLY = {"degrees_from_center": 16, "degrees_from_45": -5, "psi": 28}
+
+Z3_S6_FRIENDLY = {"degrees_from_center": -10, "degrees_from_45": -10, "psi": 35}
+Z3_S7_FRIENDLY = {"degrees_from_center": -2, "degrees_from_45": -10, "psi": 35}
+Z3_S8_FRIENDLY = {"degrees_from_center": 4, "degrees_from_45": -10, "psi": 35}
+Z3_S9_FRIENDLY = {"degrees_from_center": 17, "degrees_from_45": -10, "psi": 35}
+
+Z1_S1_ADVERSARY = {"degrees_from_center": -10, "degrees_from_45": -10, "psi": 55}
+Z1_S2_ADVERSARY = {"degrees_from_center": 8, "degrees_from_45": -10, "psi": 55}
+
+Z2_S3_ADVERSARY = {"degrees_from_center": -14, "degrees_from_45": -15, "psi": 55}
+Z2_S4_ADVERSARY = {"degrees_from_center": 0, "degrees_from_45": -15, "psi": 55}
+Z2_S5_ADVERSARY = {"degrees_from_center": 16, "degrees_from_45": -15, "psi": 55}
+
+Z3_S6_ADVERSARY = {"degrees_from_center": -10, "degrees_from_45": -15, "psi": 50}
+Z3_S7_ADVERSARY = {"degrees_from_center": -2, "degrees_from_45": -15, "psi": 50}
+Z3_S8_ADVERSARY = {"degrees_from_center": 4, "degrees_from_45": -15, "psi": 50}
+Z3_S9_ADVERSARY = {"degrees_from_center": 17, "degrees_from_45": -15, "psi": 50}

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_ground_station/src/ground_station.py

@@ -0,0 +1,198 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+import sys
+from PyQt5 import QtWidgets, QtCore, QtGui, uic
+import signal
+import rospy
+import logging
+import qdarkstyle
+import time
+
+
+# Custom Imports
+import Framework.StartupSystems.ROSMasterChecker as ROSMasterChecker
+import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator
+import Framework.InputSystems.JoystickControlSender as JoystickControlSender
+import Framework.StatusSystems.StatusCore as StatusCore
+import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
+import Framework.ArmControlSystems.ArmControlSender as ArmControlSender
+import Framework.GameSystems.GameManagerCore as GameManagerCore
+
+#####################################
+# 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)
+
+        # ########## 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("denso_ground_station")
+
+        # ##### Instantiate Regular Classes ######
+
+        # ##### Instantiate Threaded Classes ######
+        self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
+        self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects))
+        self.__add_thread("Rover Status", StatusCore.SensorCore(self.shared_objects))
+        self.__add_thread("Spacenav Sender", SpaceNavControlSender.SpaceNavControlSender(self.shared_objects))
+        self.__add_thread("Arm Control Sender", ArmControlSender.ArmControlSender(self.shared_objects))
+        self.__add_thread("Game Manager", GameManagerCore.GameManager(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 __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()
+
+        start_time = time.time()
+        while True:
+            threads_running = False
+
+            for thread in self.shared_objects["threaded_classes"]:
+                if self.shared_objects["threaded_classes"][thread].isRunning():
+                    threads_running = True
+
+            if not threads_running:
+                break
+
+            if (time.time() - start_time) > 4:
+                for thread in self.shared_objects["threaded_classes"]:
+                    if self.shared_objects["threaded_classes"][thread].isRunning():
+                        self.shared_objects["threaded_classes"][thread].terminate()
+                break
+
+        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

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_interface_controller/CMakeLists.txt

@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(denso_interface_controller)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+add_message_files(
+    FILES
+    InterfaceControlMessage.msg
+    InterfaceStatusMessage.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES system_statuses
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/system_statuses.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/system_statuses_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_system_statuses.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_interface_controller/msg/InterfaceControlMessage.msg

@@ -0,0 +1,6 @@
+float32 delay_before_processing
+
+uint8 set_pressure
+bool should_tamp
+bool should_fire
+

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_interface_controller/msg/InterfaceStatusMessage.msg

@@ -0,0 +1,7 @@
+float32 current_actual_pressure
+float32 current_set_pressure
+
+bool compressor_on
+bool ball_detected
+
+bool motor_power_on

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_interface_controller/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>denso_interface_controller</name>
+  <version>0.0.0</version>
+  <description>The denso_interface_controller package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="matcurtay@matcurtay.net">matcurtay</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rover_status</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <build_depend>message_generation</build_depend>
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <exec_depend>message_runtime</exec_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_interface_controller/src/control_tester.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import rospy
+import random
+import time
+from denso_interface_controller.msg import InterfaceControlMessage
+from std_msgs.msg import UInt8MultiArray
+
+STATUSES_TOPIC = "denso_interface_controller/control"
+
+rospy.init_node("tester")
+
+publisher = rospy.Publisher(STATUSES_TOPIC, InterfaceControlMessage, queue_size=1)
+
+message = InterfaceControlMessage()
+i = 0
+
+time.sleep(2)
+
+# while i < 10:
+#     message.delay_before_processing = 1.0
+#
+#     message.set_pressure = random.randint(0, 50)
+#     message.should_fire = 1 if random.randint(0, 20) == 0 else 0
+#     message.should_tamp = 1 if random.randint(0, 1) == 0 else 0
+#     message.delay_before_processing = random.randint(0, 1)
+#
+#     publisher.publish(message)
+#
+#     time.sleep(1)
+#     i += 1
+
+message.set_pressure = 0
+message.should_fire = 0
+
+publisher.publish(message)
+
+# time.sleep(10)
+#
+#
+# message.set_pressure = 0
+# message.should_fire = 1
+#
+# publisher.publish(message)
+
+input("Press Enter")
+

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_interface_controller/src/interface_controller.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+from time import time, sleep
+import serial
+import random
+
+from denso_interface_controller.msg import InterfaceControlMessage, InterfaceStatusMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "denso_interface_controller"
+DEFAULT_HERTZ = 30
+
+STATUSES_TOPIC = "denso_interface_controller/status"
+CONTROL_TOPIC = "denso_interface_controller/control"
+
+DEFAULT_PORT = "/dev/ttyUSB0"
+DEFAULT_BAUD = 57600
+
+
+#####################################
+# ControlSender Class Definition
+#####################################
+class InterfaceController(object):
+    def __init__(self):
+        super(InterfaceController, self).__init__()
+
+        rospy.init_node(NODE_NAME)
+
+        self.port = rospy.get_param("~port", DEFAULT_PORT)
+        self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
+
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        self.serial_device = serial.Serial(port=self.port, baudrate=self.baud)
+
+        self.status_publisher = rospy.Publisher(STATUSES_TOPIC, InterfaceStatusMessage, queue_size=1)
+        self.control_subscriber = rospy.Subscriber(CONTROL_TOPIC, InterfaceControlMessage, self.control_requested_callback)
+
+        self.command_queue = []
+
+        # ########### Start class ##########
+        self.run()
+
+    def run(self):
+
+        while not rospy.is_shutdown():
+            start_time = time()
+
+            self.broadcast_status_if_available()
+            self.send_control_packet_if_command_available()
+
+            time_diff = time() - start_time
+            sleep(max(self.wait_time - time_diff, 0))
+
+    def broadcast_status_if_available(self):
+        if self.serial_device.inWaiting():
+
+            try:
+                line = self.serial_device.readline()
+
+                blocks = line.strip("\r\n").split("|")
+
+                status_message = InterfaceStatusMessage()
+                status_message.current_actual_pressure = float(blocks[0])
+                status_message.current_set_pressure = float(blocks[1])
+                status_message.compressor_on = True if blocks[2] == '1' else False
+                status_message.ball_detected = True if blocks[3] == '1' else False
+                status_message.motor_power_on = True if blocks[4] == '1' else False
+                self.status_publisher.publish(status_message)
+
+            except:
+                print "Could not parse microcontroller message."
+
+    def send_control_packet_if_command_available(self):
+        if self.command_queue:
+            item = self.command_queue[0]  # type: InterfaceControlMessage
+            del self.command_queue[0]
+
+            psi_value = int(item.set_pressure)
+            should_tamp = int(item.should_tamp)
+            should_shoot = int(item.should_fire)
+
+            output = "%d|%d|%d|" % (psi_value, should_tamp, should_shoot)
+
+            output += "\n"
+
+            print "output:", output
+
+            self.serial_device.write(output)
+
+    def control_requested_callback(self, data):
+        self.command_queue.append(data)
+
+if __name__ == "__main__":
+    InterfaceController()

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_interface_controller/src/led_controller.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+from time import time, sleep
+import serial
+import random
+import json
+
+from std_msgs.msg import UInt8MultiArray
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "denso_led_controller"
+DEFAULT_HERTZ = 50
+
+CONTROL_TOPIC = "denso_led_controller/control"
+
+DEFAULT_PORT = "/dev/ttyACM0"
+DEFAULT_BAUD = 57600
+
+
+#####################################
+# ControlSender Class Definition
+#####################################
+class InterfaceController(object):
+    def __init__(self):
+        super(InterfaceController, self).__init__()
+
+        rospy.init_node(NODE_NAME)
+
+        self.port = rospy.get_param("~port", DEFAULT_PORT)
+        self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
+
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        self.serial_device = serial.Serial(port=self.port, baudrate=self.baud)
+
+        self.control_subscriber = rospy.Subscriber(CONTROL_TOPIC, UInt8MultiArray, self.control_requested_callback)
+
+        self.command_queue = []
+
+        temp_arary = []
+        for _ in range(18):
+            temp_arary.append(chr(255))
+            temp_arary.append(chr(165))
+            temp_arary.append(chr(0))
+
+        self.command_queue.append(UInt8MultiArray(data=temp_arary))
+
+        # ########### Start class ##########
+        self.run()
+
+    def run(self):
+
+
+        while not rospy.is_shutdown():
+            start_time = time()
+
+            # if self.serial_device.inWaiting():
+            #     print self.serial_device.readline()
+
+            self.send_control_packet_if_command_available()
+
+            time_diff = time() - start_time
+            sleep(max(self.wait_time - time_diff, 0))
+
+    def send_control_packet_if_command_available(self):
+        if self.command_queue:
+            item = self.command_queue[0]
+            del self.command_queue[0]
+
+            output = {
+                "leds": [ord(i) for i in item.data]
+            }
+
+            # print json.dumps(output)
+            self.serial_device.write(json.dumps(output) + "\n")
+
+    def control_requested_callback(self, data):
+        self.command_queue.append(data)
+
+if __name__ == "__main__":
+    InterfaceController()

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_interface_controller/src/led_tester.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+import rospy
+import random
+import time
+from std_msgs.msg import UInt8MultiArray
+
+STATUSES_TOPIC = "denso_led_controller/control"
+
+rospy.init_node("tester_led")
+
+publisher = rospy.Publisher(STATUSES_TOPIC, UInt8MultiArray, queue_size=1)
+
+message = UInt8MultiArray()
+
+i = 0
+
+while i < 1000:
+    message.data = [random.randint(0, 255) for _ in range(23*3)]
+
+    publisher.publish(message)
+
+    # time.sleep(0.01)
+    i += 1

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_main/CMakeLists.txt

@@ -0,0 +1,197 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(denso_main)
+
+## 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 denso_main
+#  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}/denso_main.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/denso_main_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_denso_main.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)

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_main/launch/denso.launch

@@ -0,0 +1,7 @@
+<launch>
+    <!-- ########## Start Denso Control Nodes ########## -->
+    <include file="$(find denso_main)/launch/denso/control.launch"/>
+
+    <!-- ########## Start ZED ########## -->
+    <include file="$(find zed_wrapper)/launch/zed.launch"/>=
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_main/launch/denso/control.launch

@@ -0,0 +1,15 @@
+<launch>
+
+    <node name="denso_control_sender" pkg="denso_master" type="control_sender.py" respawn="true" output="screen">
+    </node>
+
+    <node name="denso_status_receiver" pkg="denso_master" type="status_receiver.py" respawn="true" output="screen">
+    </node>
+
+    <node name="denso_interface_controller_main" pkg="denso_interface_controller" type="interface_controller.py" respawn="true" output="screen">
+    </node>
+
+    <node name="denso_interface_controller_led" pkg="denso_interface_controller" type="led_controller.py" respawn="true" output="screen">
+    </node>
+
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_main/launch/ground_station.launch

@@ -0,0 +1,4 @@
+<launch>
+    <!-- ########## Start Ground Station Interface ########## -->
+    <node name="denso_ground_station" pkg="denso_ground_station" type="ground_station_launch.sh" required="true" output="screen"/>
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_main/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>denso_main</name>
+  <version>0.0.0</version>
+  <description>The denso_main 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/denso_main</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>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_master/CMakeLists.txt

@@ -0,0 +1,200 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(denso_master)
+
+## 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
+  std_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  DensoStatusMessage.msg
+#   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
+)
+
+################################################
+## 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 network_master
+   CATKIN_DEPENDS rospy std_msgs message_runtime
+#  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}/network_master.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/network_master_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_network_master.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)

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_master/msg/DensoStatusMessage.msg

@@ -0,0 +1,11 @@
+float32[] positions
+float32[] joints
+
+bool motor_enabled
+bool arm_normal
+bool arm_busy
+string error
+
+uint8 speed
+
+uint8 tank_psi

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_master/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>denso_master</name>
+  <version>0.0.0</version>
+  <description>The network_master package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="denso@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/network_master</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_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_master/src/control_sender.py

@@ -0,0 +1,121 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+from time import time, sleep
+import socket
+import json
+
+from network_master.msg import DensoStatusMessage
+from std_msgs.msg import Float32MultiArray, Bool, UInt8
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "robot_control_control_sender"
+DEFAULT_HERTZ = 100
+
+DEFAULT_IP = "192.168.1.5"
+DEFAULT_PORT = 9877
+
+ABS_POS_TOPIC = "denso_control/absolute_position"
+REL_POS_TOPIC = "denso_control/relative_position"
+ABS_JOINT_TOPIC = "denso_control/absolute_joints"
+REL_JOINT_TOPIC = "denso_control/relative_joints"
+
+SPEED_TOPIC = "denso_control/speed"
+MOTOR_TOPIC = "denso_control/motors_enabled"
+
+
+#####################################
+# ControlSender Class Definition
+#####################################
+class ControlSender(object):
+    def __init__(self):
+        super(ControlSender, self).__init__()
+
+        rospy.init_node(NODE_NAME)
+
+        self.address = rospy.get_param("~server_address", DEFAULT_IP)
+        self.port = rospy.get_param("~server_port", DEFAULT_PORT)
+
+        self.abs_position_topic = rospy.get_param("~abs_position_topic_sub", ABS_POS_TOPIC)
+        self.rel_position_topic = rospy.get_param("~rel_position_topic_sub", REL_POS_TOPIC)
+        self.abs_joint_topic = rospy.get_param("~abs_joint_topic_sub", ABS_JOINT_TOPIC)
+        self.rel_joint_topic = rospy.get_param("~rel_joint_topic_sub", REL_JOINT_TOPIC)
+
+        self.speed_topic = rospy.get_param("~speed_topic_sub", SPEED_TOPIC)
+        self.motor_topic = rospy.get_param("~motor_control_topic_sub", MOTOR_TOPIC)
+
+        self.abs_position_subscriber = rospy.Subscriber(self.abs_position_topic, Float32MultiArray, self.abs_position_callback)
+        self.rel_position_subscriber = rospy.Subscriber(self.rel_position_topic, Float32MultiArray, self.rel_position_callback)
+        self.abs_joints_subscriber = rospy.Subscriber(self.abs_joint_topic, Float32MultiArray, self.abs_joints_callback)
+        self.rel_joints_subscriber = rospy.Subscriber(self.rel_joint_topic, Float32MultiArray, self.rel_joints_callback)
+
+        self.speed_subscriber = rospy.Subscriber(self.speed_topic, UInt8, self.speed_control_callback)
+        self.speed_subscriber = rospy.Subscriber(self.motor_topic, Bool, self.motor_control_callback)
+
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        self.control_tcp_client = None
+
+        self.command_queue = []
+
+        # ########### Start class ##########
+        self.run()
+
+    def run(self):
+        self.initalize_tcp_client()
+
+        while not rospy.is_shutdown():
+            start_time = time()
+
+            self.process_command_queue_item()
+
+            time_diff = time() - start_time
+            sleep(max(self.wait_time - time_diff, 0))
+
+    def initalize_tcp_client(self):
+        self.control_tcp_client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.control_tcp_client.connect((self.address, self.port))
+
+    def process_command_queue_item(self):
+        if self.command_queue:
+            data = self.command_queue[0]
+            del self.command_queue[0]
+
+            try:
+                self.control_tcp_client.sendall(json.dumps(data))
+                self.control_tcp_client.sendall("#####")
+                # print "sending"
+            except:
+                pass
+
+    def speed_control_callback(self, data):
+        # print data.data
+        self.command_queue.append({"change_robot_speed": data.data})
+
+    def motor_control_callback(self, data):
+        self.command_queue.append({"enable_motors": data.data})
+
+    def abs_position_callback(self, data):
+        # print data.data
+        self.command_queue.append({"move_position_abs": data.data})
+
+    def rel_position_callback(self, data):
+        # print data.data
+        self.command_queue.append({"move_position_rel": data.data})
+
+    def abs_joints_callback(self, data):
+        # print data.data
+        self.command_queue.append({"move_joint_abs": data.data})
+
+    def rel_joints_callback(self, data):
+        # print data.data
+        self.command_queue.append({"move_joint_rel": data.data})
+
+
+if __name__ == "__main__":
+    ControlSender()

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_master/src/move_test.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import Float32MultiArray, UInt8
+import time
+import random
+
+rospy.init_node("another_test")
+
+pub = rospy.Publisher("/denso_control/relative_joints", Float32MultiArray, queue_size=1)
+
+message = Float32MultiArray()
+
+time.sleep(2)
+
+
+while not rospy.is_shutdown():
+
+
+    message.data = (0, 0, 0.25, 0, 0, 0) # Catch
+
+    pub.publish(message)
+
+    time.sleep(3)
+

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_master/src/socket_test.py

@@ -0,0 +1,12 @@
+import rospy
+from std_msgs.msg import Float32MultiArray
+
+rospy.init_node("ano")
+
+pub = rospy.Publisher("/denso_control/relative_joints", Float32MultiArray, queue_size=1)
+
+message = Float32MultiArray()
+
+message.data = (20, 0, 0, 0, 0, 0)
+
+pub.publish(message)

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/denso_master/src/status_receiver.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+from time import time, sleep
+import socket
+import json
+
+from network_master.msg import DensoStatusMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "robot_control_status_receiver"
+DEFAULT_HERTZ = 1000
+
+DEFAULT_IP = "192.168.1.5"
+DEFAULT_PORT = 9876
+
+DEFAULT_STATUS_TOPIC = "denso_status"
+
+
+#####################################
+# ControlSender Class Definition
+#####################################
+class StatusReceiver(object):
+    def __init__(self):
+        super(StatusReceiver, self).__init__()
+
+        rospy.init_node(NODE_NAME)
+
+        self.address = rospy.get_param("~server_address", DEFAULT_IP)
+        self.port = rospy.get_param("~server_port", DEFAULT_PORT)
+        self.status_publish_topic = rospy.get_param("~status_topic_pub", DEFAULT_STATUS_TOPIC)
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        self.status_publisher = rospy.Publisher(self.status_publish_topic, DensoStatusMessage, queue_size=1)
+
+        self.status_tcp_client = None
+        self.pound_count = 0
+
+        self.current_message = ""
+
+        # ########### Start class ##########
+        self.run()
+
+    def run(self):
+        self.initalize_tcp_client()
+
+        while not rospy.is_shutdown():
+            start_time = time()
+
+            self.current_message += self.status_tcp_client.recv(4096)
+
+            found_pound = self.current_message.find("#####")
+
+            if found_pound != -1:
+                split_message = str(self.current_message[:found_pound])
+
+                self.current_message = self.current_message[found_pound + 5:]
+
+                try:
+                    json_message = json.loads(split_message)
+
+                    message = DensoStatusMessage()
+
+                    message.positions = (
+                        json_message["position"]["x"],
+                        json_message["position"]["y"],
+                        json_message["position"]["z"],
+                        json_message["position"]["rx"],
+                        json_message["position"]["ry"],
+                        json_message["position"]["rz"],
+                        json_message["position"]["fig"]
+
+                    )
+
+                    message.joints = (
+                        json_message["joints"]["1"],
+                        json_message["joints"]["2"],
+                        json_message["joints"]["3"],
+                        json_message["joints"]["4"],
+                        json_message["joints"]["5"],
+                        json_message["joints"]["6"],
+                    )
+
+                    message.motor_enabled = json_message["statuses"]["motor_enabled"]
+                    message.arm_normal = json_message["statuses"]["arm_normal"]
+                    message.error = json_message["statuses"]["error"]
+                    message.speed = json_message["statuses"]["speed"]
+                    message.arm_busy = json_message["statuses"]["arm_busy"]
+                    message.tank_psi = json_message["statuses"]["tank_psi"]
+
+                    self.status_publisher.publish(message)
+
+                except Exception, e:
+                    print e
+
+                self.current_message = ""
+
+            time_diff = time() - start_time
+            sleep(max(self.wait_time - time_diff, 0))
+
+    def initalize_tcp_client(self):
+        self.status_tcp_client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.status_tcp_client.connect((self.address, self.port))
+
+
+if __name__ == '__main__':
+    StatusReceiver()

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/CMakeLists.txt

@@ -0,0 +1,147 @@
+cmake_minimum_required(VERSION 2.8.7)
+
+project(zed_wrapper)
+
+# if CMAKE_BUILD_TYPE is not specified, take 'Release' as default
+IF(NOT CMAKE_BUILD_TYPE)
+    SET(CMAKE_BUILD_TYPE Release)
+ENDIF(NOT CMAKE_BUILD_TYPE)
+
+function(checkPackage package customMessage)
+    set(varName "${package}_FOUND")
+    if (NOT "${${varName}}")
+        string(REPLACE "_" "-" aptPackage ${package})
+        if("${customMessage}" STREQUAL "")
+            message(FATAL_ERROR "\n\n ${package} is missing, please try to install it with:\n sudo apt-get install ros-$(rosversion -d)-${aptPackage}\n\n")
+        else()
+            message(FATAL_ERROR "\n\n ${customMessage} \n\n")
+        endif()
+    endif()
+endfunction(checkPackage)
+
+find_package(ZED 2.3)
+checkPackage("ZED" "ZED SDK not found, install it from:\n https://www.stereolabs.com/developers/")
+
+exec_program(uname ARGS -p OUTPUT_VARIABLE CMAKE_SYSTEM_NAME2)
+if ( CMAKE_SYSTEM_NAME2 MATCHES "aarch64" ) # Jetson TX
+    SET(CUDA_USE_STATIC_CUDA_RUNTIME OFF)
+endif()
+
+find_package(OpenCV 3 COMPONENTS core highgui imgproc)
+checkPackage("OPENCV_CORE" "OpenCV core not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
+checkPackage("OPENCV_HIGHGUI" "OpenCV highgui not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
+checkPackage("OPENCV_IMGPROC" "OpenCV imgproc not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
+
+find_package(CUDA)
+checkPackage("CUDA" "CUDA not found, install it from:\n https://developer.nvidia.com/cuda-downloads")
+
+find_package(PCL)
+checkPackage("PCL" "PCL not found, try to install it with:\n sudo apt-get install libpcl1 ros-$(rosversion -d)-pcl-ros")
+
+find_package(catkin COMPONENTS
+  image_transport
+  roscpp
+  rosconsole
+  sensor_msgs
+  dynamic_reconfigure
+  tf2_ros
+  pcl_conversions
+  nodelet
+  tf2_geometry_msgs
+)
+
+checkPackage("image_transport" "")
+checkPackage("roscpp" "")
+checkPackage("rosconsole" "")
+checkPackage("sensor_msgs" "")
+checkPackage("dynamic_reconfigure" "")
+checkPackage("tf2_ros" "")
+checkPackage("pcl_conversions" "")
+checkPackage("nodelet" "")
+checkPackage("tf2_geometry_msgs" "")
+
+generate_dynamic_reconfigure_options(
+  cfg/Zed.cfg
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    roscpp
+    rosconsole
+    sensor_msgs
+    opencv
+    image_transport
+    dynamic_reconfigure
+    tf2_ros
+    tf2_geometry_msgs
+    pcl_conversions
+)
+###############################################################################
+# INCLUDES
+
+# Specify locations of header files.
+include_directories(
+	${catkin_INCLUDE_DIRS}
+	${CUDA_INCLUDE_DIRS}
+	${ZED_INCLUDE_DIRS}
+	${OpenCV_INCLUDE_DIRS}
+	${PCL_INCLUDE_DIRS}
+)
+
+link_directories(${ZED_LIBRARY_DIR})
+link_directories(${CUDA_LIBRARY_DIRS})
+link_directories(${OpenCV_LIBRARY_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+
+###############################################################################
+
+###############################################################################
+# EXECUTABLE
+
+
+if(NOT DEFINED CUDA_NPP_LIBRARIES_ZED)
+    #To deal with cuda 9 nppi libs and previous versions of ZED SDK
+    set(CUDA_NPP_LIBRARIES_ZED ${CUDA_npp_LIBRARY} ${CUDA_npps_LIBRARY} ${CUDA_nppi_LIBRARY})   
+endif()
+
+
+add_definitions(-std=c++11)
+list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") ## if using from pcl package (pcl/vtk bug)
+set(LINK_LIBRARIES
+  ${catkin_LIBRARIES}
+  ${ZED_LIBRARIES}
+  ${CUDA_LIBRARIES} ${CUDA_NPP_LIBRARIES_ZED}
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES})
+
+add_library(ZEDWrapper src/zed_wrapper_nodelet.cpp)
+target_link_libraries(ZEDWrapper ${LINK_LIBRARIES})
+add_dependencies(ZEDWrapper ${PROJECT_NAME}_gencfg)
+
+add_executable(zed_wrapper_node src/zed_wrapper_node.cpp)
+target_link_libraries(zed_wrapper_node ZEDWrapper ${LINK_LIBRARIES})
+add_dependencies(zed_wrapper_node ${PROJECT_NAME}_gencfg)
+
+###############################################################################
+
+#Add all files in subdirectories of the project in
+# a dummy_target so qtcreator have access to all files
+FILE(GLOB_RECURSE extra_files ${CMAKE_SOURCE_DIR}/*)
+add_custom_target(dummy_${PROJECT_NAME} SOURCES ${extra_files})
+
+###############################################################################
+# INSTALL
+
+install(TARGETS
+  ZEDWrapper
+  zed_wrapper_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(FILES
+  nodelet_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY
+  launch
+  urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/LICENSE

@@ -0,0 +1,28 @@
+Copyright (c) 2017, Stereolabs
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of zed-ros-wrapper nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/README.md

@@ -0,0 +1,61 @@
+# Stereolabs ZED Camera - ROS Integration
+
+This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, odometry information and supports the use of multiple ZED cameras.
+
+## Getting started
+
+- First, download the latest version of the ZED SDK on [stereolabs.com](https://www.stereolabs.com/developers/)
+- [Install](#build-the-program) the ZED ROS wrapper.
+- For more information, check out our [ROS documentation](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html) or our [ROS wiki](http://wiki.ros.org/zed-ros-wrapper). If you want to customize the wrapper, check the [ZED API documentation](https://www.stereolabs.com/developers/documentation/API/).
+
+### Prerequisites
+
+- Ubuntu 16.04
+- [ZED SDK **≥ 2.3**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads)
+- [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu)
+- [Point Cloud Library (PCL)](https://github.com/PointCloudLibrary/pcl)
+
+### Build the program
+
+The zed_ros_wrapper is a catkin package. It depends on the following ROS packages:
+
+   - tf2_ros
+   - tf2_geometry_msgs
+   - nav_msgs
+   - roscpp
+   - rosconsole
+   - sensor_msgs
+   - opencv
+   - image_transport
+   - dynamic_reconfigure
+   - urdf
+
+
+Open a terminal and build the package:
+
+    cd ~/catkin_ws/src
+    git clone https://github.com/stereolabs/zed-ros-wrapper.git
+    cd ../
+    catkin_make
+    source ./devel/setup.bash
+
+### Run the program
+
+To launch the wrapper along with an Rviz preview, open a terminal and launch:
+
+    roslaunch zed_wrapper display.launch # by default open a ZED
+
+or
+
+    roslaunch zed_wrapper display_zedm.launch # open a ZED Mini
+
+
+To launch the wrapper without Rviz, use:
+
+    roslaunch zed_wrapper zed.launch
+
+ To select the ZED from its serial number
+
+    roslaunch zed_wrapper zed.launch serial_number:=1010 #replace 1010 with the actual SN
+
+[More](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html)

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/cfg/Zed.cfg

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+PACKAGE = "zed_wrapper"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 80,  1, 100)
+gen.add("exposure", int_t, 1, "Exposure value when manual controlled", 100, 0, 100);
+gen.add("gain", int_t, 2, "Gain value when manual controlled", 50, 0, 100);
+gen.add("auto_exposure", bool_t, 3, "Enable/Disable auto control of exposure and gain", True);
+
+exit(gen.generate(PACKAGE, "zed_wrapper", "Zed"))

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/launch/README.md

@@ -0,0 +1,27 @@
+# Launch Files
+
+Launch files provide a convenient way to start up multiple nodes and a master, as well as setting parameters.
+
+Use **roslaunch** to open ZED launch files.
+```
+roslaunch zed_wrapper zed.launch
+```
+
+### Start the node with one ZED
+Use the **zed.launch** file to launch a single camera nodelet. It includes one instance of **zed\_camera.launch**.
+```
+roslaunch zed_wrapper zed.launch
+```
+
+### Start the node with multiple ZED
+To use multiple ZED, launch the **zed\_multi\_cam.launch** file which describes the different cameras. This example includes two instances of **zed\_camera.launch**.
+```
+roslaunch zed_wrapper zed_multi_cam.launch
+```
+
+### Start the node with multiple ZED and GPUs
+You can configure the wrapper to assign a GPU to a ZED. In that case, it it is not possible to use several instances of **zed\_camera.launch** because different parameters need to be set for each ZED.
+A sample **zed\_multi\_gpu.launch** file is available to show how to work with different ZED and GPUs.
+```
+roslaunch zed_wrapper zed_multi_gpu.launch
+```

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/launch/display.launch

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2017, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+    
+  <!-- Launch ZED camera wrapper -->
+  <include file="$(find zed_wrapper)/launch/zed.launch">
+      <arg name="camera_model" value="0" /> <!-- 0=ZED, 1=ZEDM-->
+  </include>
+  
+  <!-- Launch rivz display -->
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find zed_wrapper)/rviz/zed.rviz" output="screen" />
+
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/launch/display_zedm.launch

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2017, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+    
+  <!-- Launch ZED camera wrapper -->
+  <include file="$(find zed_wrapper)/launch/zed.launch">
+      <arg name="camera_model" value="1" /> <!-- 0=ZED, 1=ZEDM-->
+  </include>
+  
+  <!-- Launch rivz display -->
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find zed_wrapper)/rviz/zedm.rviz" output="screen" />
+
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/launch/zed.launch

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2017, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+    <!-- Odometry coordinate frame -->
+    <arg name="odometry_frame"       default="map" />
+    <arg name="camera_model"          default="0" /> <!-- 0=ZED, 1=ZEDM-->
+    <arg name="serial_number"           default="0" /> 
+
+    <group ns="zed">
+        <include file="$(find zed_wrapper)/launch/zed_camera.launch">
+            <!-- compliant mode for rviz -->
+            <arg name="odometry_frame"        value="$(arg odometry_frame)" />
+            <arg name="camera_model"        value="$(arg camera_model)" />
+            <arg name="serial_number"           default="$(arg serial_number)" /> 
+        </include>
+    </group>
+
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/launch/zed_camera.launch

@@ -0,0 +1,102 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2017, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+    <arg name="camera_model"          default="0" /> <!-- 0=ZED, 1=ZEDM-->
+    <arg name="svo_file"              default="" /><!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
+    <arg name="zed_id"                default="0" />
+    <!-- If a ZED SN is given, zed_id is ignored and the wrapper will specifically look for the ZED with the corresponding serial number -->
+    <arg name="serial_number"                default="0" /> 
+
+    <!-- GPU ID-->
+    <arg name="gpu_id"                default="-1" />
+    <!-- Definition coordinate frames -->
+    <arg name="publish_tf"            default="true" />
+    <arg name="odometry_frame"        default="odom" />
+    <arg name="base_frame"            default="zed_center" />
+    <arg name="camera_frame"          default="zed_left_camera" />
+    <arg name="depth_frame"           default="zed_depth_camera" />
+    <!-- Publish urdf zed -->
+    <arg name="publish_urdf"          default="true" />
+    
+    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" --> 
+
+        <param name="camera_model" value="$(arg camera_model)" />
+        <!-- publish odometry frame -->
+        <param name="publish_tf"            value="$(arg publish_tf)" />
+        <!-- Configuration frame camera -->    
+        <param name="odometry_frame"        value="$(arg odometry_frame)" />
+        <param name="base_frame"            value="$(arg base_frame)" />
+        <param name="camera_frame"          value="$(arg camera_frame)" />
+        <param name="depth_frame"           value="$(arg depth_frame)" />
+    
+        <!-- SVO file path -->
+        <param name="svo_filepath"          value="$(arg svo_file)" />
+
+        <!-- ZED parameters -->
+        <param name="zed_id"                value="$(arg zed_id)" />
+        <param name="serial_number"      value="$(arg serial_number)" />
+
+        <param name="resolution"            value="2" />
+        <param name="quality"               value="1" />
+        <param name="sensing_mode"          value="0" />
+        <param name="frame_rate"            value="60" />
+        <param name="odometry_db"           value="" />
+        <param name="openni_depth_mode"     value="0" />
+        <param name="gpu_id"                value="$(arg gpu_id)" />
+        <param name="confidence"            value="100" />
+        <param name="gain"                  value="100" />
+        <param name="exposure"              value="100" />
+        <param name="auto_exposure"         value="true" />
+        <param name="depth_stabilization"   value="1" />
+
+        <!-- ROS topic names -->
+        <param name="rgb_topic"             value="rgb/image_rect_color" />
+        <param name="rgb_raw_topic"         value="rgb/image_raw_color" />
+        <param name="rgb_cam_info_topic"    value="rgb/camera_info" />
+        <param name="rgb_cam_info_raw_topic"    value="rgb/camera_info_raw" />
+
+        <param name="left_topic"            value="left/image_rect_color" />
+        <param name="left_raw_topic"        value="left/image_raw_color" />
+        <param name="left_cam_info_topic"   value="left/camera_info" />
+        <param name="left_cam_info_raw_topic"   value="left/camera_info_raw" />
+
+        <param name="right_topic"           value="right/image_rect_color" />
+        <param name="right_raw_topic"       value="right/image_raw_color" />
+        <param name="right_cam_info_topic"  value="right/camera_info" />
+        <param name="right_cam_info_raw_topic"  value="right/camera_info_raw" />
+
+        <param name="depth_topic"           value="depth/depth_registered" />
+        <param name="depth_cam_info_topic"  value="depth/camera_info" />
+
+        <param name="point_cloud_topic"     value="point_cloud/cloud_registered" />
+
+        <param name="odometry_topic"        value="odom" />
+
+    </node>
+
+    <!-- ROS URDF description of the ZED -->
+    <group if="$(arg publish_urdf)">
+        <param if="$(arg camera_model)" name="zed_description" textfile="$(find zed_wrapper)/urdf/zedm.urdf" />
+        <param unless="$(arg camera_model)" name="zed_description" textfile="$(find zed_wrapper)/urdf/zed.urdf" />
+        <node name="zed_state_publisher" pkg="robot_state_publisher" type="state_publisher">
+            <remap from="robot_description" to="zed_description" />
+        </node>
+    </group>
+        
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/launch/zed_multi_cam.launch

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2017, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+
+    <!-- Odometry coordinate frame -->
+    <arg name="odometry_frame"        default="map" />
+
+    <arg name="zed_id1"               default="0" />
+    <arg name="zed_id2"               default="1" />
+
+    <!-- First ZED camera -->
+    <group ns="zed1">
+        <include file="$(find zed_wrapper)/launch/zed_camera.launch">
+            <arg name="zed_id"                value="$(arg zed_id1)" />
+            <!-- compliant mode for rviz -->
+            <arg name="odometry_frame"        value="$(arg odometry_frame)" />
+        </include>
+    </group>
+
+    <!-- Second ZED camera -->
+    <group ns="zed2">
+        <include file="$(find zed_wrapper)/launch/zed_camera.launch">
+            <arg name="zed_id"                value="$(arg zed_id2)" />
+            <!-- compliant mode for rviz -->
+            <arg name="odometry_frame"        value="$(arg odometry_frame)" />
+        </include>
+    </group>
+
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/launch/zed_multi_gpu.launch

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2017, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+
+    <!-- Odometry coordinate frame -->
+    <arg name="odometry_frame"        default="map" />
+
+    <arg name="zed_id1"               default="0" />
+    <arg name="zed_id2"               default="1" />
+
+    <!-- First ZED camera on GPU 0 -->
+    <group ns="zed_GPU0">
+        <include file="$(find zed_wrapper)/launch/zed_camera.launch">
+            <arg name="zed_id"                value="$(arg zed_id1)" />
+            <!-- Set GPU -->
+            <arg name="gpu_id"                value="0" />
+            <!-- compliant mode for rviz -->
+            <arg name="odometry_frame"        value="$(arg odometry_frame)" />
+        </include>
+    </group>
+
+    <!-- Second ZED camera on GPU 1 -->
+    <group ns="zed_GPU1">
+        <include file="$(find zed_wrapper)/launch/zed_camera.launch">
+            <arg name="zed_id"                value="$(arg zed_id2)" />
+            <!-- Set GPU -->
+            <arg name="gpu_id"                value="1" />
+            <!-- compliant mode for rviz -->
+            <arg name="odometry_frame"        value="$(arg odometry_frame)" />
+        </include>
+    </group>
+
+</launch>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/nodelet_plugins.xml

@@ -0,0 +1,7 @@
+<library path="lib/libZEDWrapper">
+  <class name="zed_wrapper/ZEDWrapperNodelet"
+         type="zed_wrapper::ZEDWrapperNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description>This is the nodelet of ROS interface for Stereolabs ZED Camera.</description>
+  </class>
+</library>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/package.xml

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<package>
+  <name>zed_wrapper</name>
+  <version>1.0.0</version>
+  <description>zed_wrapper is a ROS wrapper for the <a href="http://www.stereolabs.com/developers/">ZED library</a>
+    for interfacing with the ZED camera.
+  </description>
+<maintainer email="developers@stereolabs.com">STEREOLABS</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rosconsole</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>opencv</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>urdf</build_depend>
+  <build_depend>nodelet</build_depend>
+
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rosconsole</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>opencv</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>pcl_conversions</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
+</package>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/records/record_depth.sh

@@ -0,0 +1,2 @@
+#!/bin/bash
+rosbag record /camera/rgb/camera_info /camera/depth_registered/image_raw/compressedDepth /camera/rgb/image_rect_color/compressed /tf

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/records/record_stereo.sh

@@ -0,0 +1,2 @@
+#!/bin/bash
+rosbag record /camera/left/camera_info /camera/right/camera_info /camera/left/image_raw/compressed /camera/right/image_raw/compressed /tf

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/rviz/zed.rviz

@@ -0,0 +1,301 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /RobotModel1/Links1
+        - /TF1/Frames1
+        - /Odometry1/Shape1
+      Splitter Ratio: 0.5
+    Tree Height: 281
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Camera
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        zed_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        zed_depth_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        zed_left_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        zed_right_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: zed/zed_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        map:
+          Value: false
+        zed_center:
+          Value: false
+        zed_depth_camera:
+          Value: false
+        zed_left_camera:
+          Value: true
+        zed_right_camera:
+          Value: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          zed_center:
+            zed_left_camera:
+              zed_depth_camera:
+                {}
+            zed_right_camera:
+              {}
+      Update Interval: 0
+      Value: true
+    - Angle Tolerance: 0
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 1
+      Name: Odometry
+      Position Tolerance: 0
+      Shape:
+        Alpha: 0
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 255; 25; 0
+        Head Length: 0.200000003
+        Head Radius: 0.100000001
+        Shaft Length: 0.300000012
+        Shaft Radius: 0.0199999996
+        Value: Arrow
+      Topic: /zed/odom
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Auto Size:
+        Auto Size Factor: 1
+        Value: true
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/DepthCloud
+      Color: 255; 255; 255
+      Color Image Topic: /zed/rgb/image_raw_color
+      Color Transformer: RGB8
+      Color Transport Hint: raw
+      Decay Time: 0
+      Depth Map Topic: /zed/depth/depth_registered
+      Depth Map Transport Hint: raw
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: DepthCloud
+      Occlusion Compensation:
+        Occlusion Time-Out: 30
+        Value: false
+      Position Transformer: XYZ
+      Queue Size: 5
+      Selectable: true
+      Size (Pixels): 3
+      Style: Flat Squares
+      Topic Filter: true
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /zed/rgb/image_rect_color
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+      Visibility:
+        DepthCloud: true
+        Grid: true
+        Odometry: true
+        PointCloud2: true
+        RobotModel: true
+        TF: true
+        Value: false
+      Zoom Factor: 1
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /zed/point_cloud/cloud_registered
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.33887124
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.00562699512
+        Y: -0.0610139929
+        Z: -0.0259050336
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.310398251
+      Target Frame: zed_left_camera
+      Value: Orbit (rviz)
+      Yaw: 3.44725299
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 793
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000028ffc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002400000010f0000000000000000fb0000000a0049006d006100670065000000028b000000c40000000000000000fb0000000a0049006d006100670065010000028a000000c50000000000000000fb0000000a0049006d00610067006500000002b30000009c0000000000000000fb0000000a0049006d006100670065010000028b000000c40000000000000000fb0000000c00430061006d00650072006101000001d6000000e10000001600ffffff000000010000010f00000327fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000327000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000045d0000003efc0100000002fb0000000800540069006d006501000000000000045d0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002ed0000028f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1117
+  X: 1474
+  Y: 432

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/rviz/zedm.rviz

@@ -0,0 +1,331 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /RobotModel1/Links1
+        - /Odometry1/Shape1
+        - /Imu1
+      Splitter Ratio: 0.5
+    Tree Height: 492
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Camera
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        zed_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        zed_depth_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        zed_imu:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        zed_left_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        zed_right_camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: zed/zed_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        map:
+          Value: false
+        zed_center:
+          Value: false
+        zed_depth_camera:
+          Value: false
+        zed_imu:
+          Value: false
+        zed_left_camera:
+          Value: true
+        zed_right_camera:
+          Value: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          zed_center:
+            zed_imu:
+              {}
+            zed_left_camera:
+              zed_depth_camera:
+                {}
+            zed_right_camera:
+              {}
+      Update Interval: 0
+      Value: true
+    - Angle Tolerance: 0
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 1
+      Name: Odometry
+      Position Tolerance: 0
+      Shape:
+        Alpha: 0
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 255; 25; 0
+        Head Length: 0.200000003
+        Head Radius: 0.100000001
+        Shaft Length: 0.300000012
+        Shaft Radius: 0.0199999996
+        Value: Arrow
+      Topic: /zed/odom
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Auto Size:
+        Auto Size Factor: 1
+        Value: true
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/DepthCloud
+      Color: 255; 255; 255
+      Color Image Topic: /zed/rgb/image_raw_color
+      Color Transformer: RGB8
+      Color Transport Hint: raw
+      Decay Time: 0
+      Depth Map Topic: /zed/depth/depth_registered
+      Depth Map Transport Hint: raw
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: DepthCloud
+      Occlusion Compensation:
+        Occlusion Time-Out: 30
+        Value: false
+      Position Transformer: XYZ
+      Queue Size: 5
+      Selectable: true
+      Size (Pixels): 3
+      Style: Flat Squares
+      Topic Filter: true
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /zed/rgb/image_rect_color
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+      Visibility:
+        DepthCloud: true
+        Grid: true
+        Imu: true
+        Odometry: true
+        PointCloud2: true
+        RobotModel: true
+        TF: true
+        Value: false
+      Zoom Factor: 1
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /zed/point_cloud/cloud_registered
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Acceleration properties:
+        Acc. vector alpha: 1
+        Acc. vector color: 255; 0; 0
+        Acc. vector scale: 1
+        Derotate acceleration: true
+        Enable acceleration: false
+      Axes properties:
+        Axes scale: 1
+        Enable axes: true
+      Box properties:
+        Box alpha: 1
+        Box color: 255; 0; 0
+        Enable box: false
+        x_scale: 1
+        y_scale: 1
+        z_scale: 1
+      Class: rviz_imu_plugin/Imu
+      Enabled: true
+      Name: Imu
+      Topic: /zed/imu_raw
+      Unreliable: false
+      Value: true
+      fixed_frame_orientation: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2.76508617
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.178178757
+        Y: -0.160461873
+        Z: 0.0447466969
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.664797008
+      Target Frame: zed_left_camera
+      Value: Orbit (rviz)
+      Yaw: 2.39227414
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1154
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003d7fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000440000027e000000d400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002400000010f0000000000000000fb0000000a0049006d006100670065000000028b000000c40000000000000000fb0000000a0049006d006100670065010000028a000000c50000000000000000fb0000000a0049006d00610067006500000002b30000009c0000000000000000fb0000000a0049006d006100670065010000028b000000c40000000000000000fb0000000c00430061006d00650072006101000002c8000001530000001800ffffff000000010000010f00000327fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000327000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000040fc0100000002fb0000000800540069006d00650100000000000007800000027100fffffffb0000000800540069006d0065010000000000000450000000000000000000000610000003d700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1920
+  X: 1920
+  Y: 360

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/src/zed_wrapper_node.cpp

@@ -0,0 +1,37 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// Copyright (c) 2017, STEREOLABS.
+//
+// All rights reserved.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+///////////////////////////////////////////////////////////////////////////
+
+#include <ros/ros.h>
+#include <nodelet/loader.h>
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "zed_wrapper_node");
+
+    nodelet::Loader nodelet;
+    nodelet::M_string remap(ros::names::getRemappings());
+    nodelet::V_string nargv;
+    nodelet.load(ros::this_node::getName(),
+            "zed_wrapper/ZEDWrapperNodelet",
+            remap, nargv);
+
+    ros::spin();
+
+    return 0;
+}

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/src/zed_wrapper_nodelet.cpp

@@ -0,0 +1,995 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// Copyright (c) 2017, STEREOLABS.
+//
+// All rights reserved.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+///////////////////////////////////////////////////////////////////////////
+
+
+
+
+/****************************************************************************************************
+ ** This sample is a wrapper for the ZED library in order to use the ZED Camera with ROS.          **
+ ** A set of parameters can be specified in the launch file.                                       **
+ ****************************************************************************************************/
+
+#include <csignal>
+#include <cstdio>
+#include <math.h>
+#include <limits>
+#include <thread>
+#include <chrono>
+#include <memory>
+#include <sys/stat.h>
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/distortion_models.h>
+#include <sensor_msgs/image_encodings.h>
+#include <image_transport/image_transport.h>
+#include <dynamic_reconfigure/server.h>
+#include <zed_wrapper/ZedConfig.h>
+#include <nav_msgs/Odometry.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include <boost/make_shared.hpp>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <sl/Camera.hpp>
+
+using namespace std;
+
+namespace zed_wrapper {
+
+    int checkCameraReady(unsigned int serial_number) {
+        int id = -1;
+        auto f = sl::Camera::getDeviceList();
+        for (auto &it : f)
+            if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE)
+                id = it.id;
+        return id;
+    }
+
+    sl::DeviceProperties getZEDFromSN(unsigned int serial_number) {
+        sl::DeviceProperties prop;
+        auto f = sl::Camera::getDeviceList();
+        for (auto &it : f) {
+            if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE)
+                prop = it;
+        }
+        return prop;
+    }
+
+    class ZEDWrapperNodelet : public nodelet::Nodelet {
+        ros::NodeHandle nh;
+        ros::NodeHandle nh_ns;
+        boost::shared_ptr<boost::thread> device_poll_thread;
+        image_transport::Publisher pub_rgb;
+        image_transport::Publisher pub_raw_rgb;
+        image_transport::Publisher pub_left;
+        image_transport::Publisher pub_raw_left;
+        image_transport::Publisher pub_right;
+        image_transport::Publisher pub_raw_right;
+        image_transport::Publisher pub_depth;
+        ros::Publisher pub_cloud;
+        ros::Publisher pub_rgb_cam_info;
+        ros::Publisher pub_left_cam_info;
+        ros::Publisher pub_right_cam_info;
+        ros::Publisher pub_rgb_cam_info_raw;
+        ros::Publisher pub_left_cam_info_raw;
+        ros::Publisher pub_right_cam_info_raw;
+        ros::Publisher pub_depth_cam_info;
+        ros::Publisher pub_odom;
+
+        // tf
+        tf2_ros::TransformBroadcaster transform_odom_broadcaster;
+        std::string left_frame_id;
+        std::string right_frame_id;
+        std::string rgb_frame_id;
+        std::string depth_frame_id;
+        std::string cloud_frame_id;
+        std::string odometry_frame_id;
+        std::string base_frame_id;
+        std::string camera_frame_id;
+        // initialization Transform listener
+        boost::shared_ptr<tf2_ros::Buffer> tfBuffer;
+        boost::shared_ptr<tf2_ros::TransformListener> tf_listener;
+        bool publish_tf;
+
+        // Launch file parameters
+        int resolution;
+        int quality;
+        int sensing_mode;
+        int rate;
+        int gpu_id;
+        int zed_id;
+        int depth_stabilization;
+        std::string odometry_DB;
+        std::string svo_filepath;
+
+        //Tracking variables
+        sl::Pose pose;
+        tf2::Transform base_transform;
+
+        // zed object
+        sl::InitParameters param;
+        sl::Camera zed;
+        unsigned int serial_number;
+
+        // flags
+        int confidence;
+        int exposure;
+        int gain;
+        bool autoExposure;
+        bool triggerAutoExposure;
+        bool computeDepth;
+        bool grabbing = false;
+        int openniDepthMode = 0; // 16 bit UC data in mm else 32F in m, for more info http://www.ros.org/reps/rep-0118.html
+
+        // Point cloud variables
+        sl::Mat cloud;
+        string point_cloud_frame_id = "";
+        ros::Time point_cloud_time;
+
+        ~ZEDWrapperNodelet() {
+            device_poll_thread.get()->join();
+        }
+
+        /* \brief Convert an sl:Mat to a cv::Mat
+         * \param mat : the sl::Mat to convert
+         */
+        cv::Mat toCVMat(sl::Mat &mat) {
+            if (mat.getMemoryType() == sl::MEM_GPU)
+                mat.updateCPUfromGPU();
+
+            int cvType;
+            switch (mat.getDataType()) {
+                case sl::MAT_TYPE_32F_C1:
+                    cvType = CV_32FC1;
+                    break;
+                case sl::MAT_TYPE_32F_C2:
+                    cvType = CV_32FC2;
+                    break;
+                case sl::MAT_TYPE_32F_C3:
+                    cvType = CV_32FC3;
+                    break;
+                case sl::MAT_TYPE_32F_C4:
+                    cvType = CV_32FC4;
+                    break;
+                case sl::MAT_TYPE_8U_C1:
+                    cvType = CV_8UC1;
+                    break;
+                case sl::MAT_TYPE_8U_C2:
+                    cvType = CV_8UC2;
+                    break;
+                case sl::MAT_TYPE_8U_C3:
+                    cvType = CV_8UC3;
+                    break;
+                case sl::MAT_TYPE_8U_C4:
+                    cvType = CV_8UC4;
+                    break;
+            }
+            return cv::Mat((int) mat.getHeight(), (int) mat.getWidth(), cvType, mat.getPtr<sl::uchar1>(sl::MEM_CPU), mat.getStepBytes(sl::MEM_CPU));
+        }
+
+        cv::Mat convertRodrigues(sl::float3 r) {
+            double theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z);
+            cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
+
+            if (theta < DBL_EPSILON) {
+                return R;
+            } else {
+                double c = cos(theta);
+                double s = sin(theta);
+                double c1 = 1. - c;
+                double itheta = theta ? 1. / theta : 0.;
+
+                r *= itheta;
+
+                cv::Mat rrt = cv::Mat::eye(3, 3, CV_32F);
+                float* p = (float*) rrt.data;
+                p[0] = r.x * r.x;
+                p[1] = r.x * r.y;
+                p[2] = r.x * r.z;
+                p[3] = r.x * r.y;
+                p[4] = r.y * r.y;
+                p[5] = r.y * r.z;
+                p[6] = r.x * r.z;
+                p[7] = r.y * r.z;
+                p[8] = r.z * r.z;
+
+                cv::Mat r_x = cv::Mat::eye(3, 3, CV_32F);
+                p = (float*) r_x.data;
+                p[0] = 0;
+                p[1] = -r.z;
+                p[2] = r.y;
+                p[3] = r.z;
+                p[4] = 0;
+                p[5] = -r.x;
+                p[6] = -r.y;
+                p[7] = r.x;
+                p[8] = 0;
+
+                // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
+                R = c * cv::Mat::eye(3, 3, CV_32F) + c1 * rrt + s*r_x;
+            }
+            return R;
+        }
+
+        /* \brief Test if a file exist
+         * \param name : the path to the file
+         */
+        bool file_exist(const std::string& name) {
+            struct stat buffer;
+            return (stat(name.c_str(), &buffer) == 0);
+        }
+
+        /* \brief Image to ros message conversion
+         * \param img : the image to publish
+         * \param encodingType : the sensor_msgs::image_encodings encoding type
+         * \param frameId : the id of the reference frame of the image
+         * \param t : the ros::Time to stamp the image
+         */
+        sensor_msgs::ImagePtr imageToROSmsg(cv::Mat img, const std::string encodingType, std::string frameId, ros::Time t) {
+            sensor_msgs::ImagePtr ptr = boost::make_shared<sensor_msgs::Image>();
+            sensor_msgs::Image& imgMessage = *ptr;
+            imgMessage.header.stamp = t;
+            imgMessage.header.frame_id = frameId;
+            imgMessage.height = img.rows;
+            imgMessage.width = img.cols;
+            imgMessage.encoding = encodingType;
+            int num = 1; //for endianness detection
+            imgMessage.is_bigendian = !(*(char *) &num == 1);
+            imgMessage.step = img.cols * img.elemSize();
+            size_t size = imgMessage.step * img.rows;
+            imgMessage.data.resize(size);
+
+            if (img.isContinuous())
+                memcpy((char*) (&imgMessage.data[0]), img.data, size);
+            else {
+                uchar* opencvData = img.data;
+                uchar* rosData = (uchar*) (&imgMessage.data[0]);
+                for (unsigned int i = 0; i < img.rows; i++) {
+                    memcpy(rosData, opencvData, imgMessage.step);
+                    rosData += imgMessage.step;
+                    opencvData += img.step;
+                }
+            }
+            return ptr;
+        }
+
+        /* \brief Publish the pose of the camera with a ros Publisher
+         * \param base_transform : Transformation representing the camera pose from base frame
+         * \param pub_odom : the publisher object to use
+         * \param odom_frame_id : the id of the reference frame of the pose
+         * \param t : the ros::Time to stamp the image
+         */
+        void publishOdom(tf2::Transform base_transform, ros::Publisher &pub_odom, string odom_frame_id, ros::Time t) {
+            nav_msgs::Odometry odom;
+            odom.header.stamp = t;
+            odom.header.frame_id = odom_frame_id; // odom_frame
+            odom.child_frame_id = base_frame_id; // base_frame
+            // conversion from Tranform to message
+            geometry_msgs::Transform base2 = tf2::toMsg(base_transform);
+            // Add all value in odometry message
+            odom.pose.pose.position.x = base2.translation.x;
+            odom.pose.pose.position.y = base2.translation.y;
+            odom.pose.pose.position.z = base2.translation.z;
+            odom.pose.pose.orientation.x = base2.rotation.x;
+            odom.pose.pose.orientation.y = base2.rotation.y;
+            odom.pose.pose.orientation.z = base2.rotation.z;
+            odom.pose.pose.orientation.w = base2.rotation.w;
+            // Publish odometry message
+            pub_odom.publish(odom);
+        }
+
+        /* \brief Publish the pose of the camera as a transformation
+         * \param base_transform : Transformation representing the camera pose from base frame
+         * \param trans_br : the TransformBroadcaster object to use
+         * \param odometry_transform_frame_id : the id of the transformation
+         * \param t : the ros::Time to stamp the image
+         */
+        void publishTrackedFrame(tf2::Transform base_transform, tf2_ros::TransformBroadcaster &trans_br, string odometry_transform_frame_id, ros::Time t) {
+            geometry_msgs::TransformStamped transformStamped;
+            transformStamped.header.stamp = ros::Time::now();
+            transformStamped.header.frame_id = odometry_frame_id;
+            transformStamped.child_frame_id = odometry_transform_frame_id;
+            // conversion from Tranform to message
+            transformStamped.transform = tf2::toMsg(base_transform);
+            // Publish transformation
+            trans_br.sendTransform(transformStamped);
+        }
+
+        /* \brief Publish a cv::Mat image with a ros Publisher
+         * \param img : the image to publish
+         * \param pub_img : the publisher object to use
+         * \param img_frame_id : the id of the reference frame of the image
+         * \param t : the ros::Time to stamp the image
+         */
+        void publishImage(cv::Mat img, image_transport::Publisher &pub_img, string img_frame_id, ros::Time t) {
+            pub_img.publish(imageToROSmsg(img, sensor_msgs::image_encodings::BGR8, img_frame_id, t));
+        }
+
+        /* \brief Publish a cv::Mat depth image with a ros Publisher
+         * \param depth : the depth image to publish
+         * \param pub_depth : the publisher object to use
+         * \param depth_frame_id : the id of the reference frame of the depth image
+         * \param t : the ros::Time to stamp the depth image
+         */
+        void publishDepth(cv::Mat depth, image_transport::Publisher &pub_depth, string depth_frame_id, ros::Time t) {
+            string encoding;
+            if (openniDepthMode) {
+                depth *= 1000.0f;
+                depth.convertTo(depth, CV_16UC1); // in mm, rounded
+                encoding = sensor_msgs::image_encodings::TYPE_16UC1;
+            } else {
+                encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+            }
+            pub_depth.publish(imageToROSmsg(depth, encoding, depth_frame_id, t));
+        }
+
+        /* \brief Publish a pointCloud with a ros Publisher
+         * \param width : the width of the point cloud
+         * \param height : the height of the point cloud
+         * \param pub_cloud : the publisher object to use
+         */
+        void publishPointCloud(int width, int height, ros::Publisher &pub_cloud) {
+            pcl::PointCloud<pcl::PointXYZRGB> point_cloud;
+            point_cloud.width = width;
+            point_cloud.height = height;
+            int size = width*height;
+            point_cloud.points.resize(size);
+
+            sl::Vector4<float>* cpu_cloud = cloud.getPtr<sl::float4>();
+            for (int i = 0; i < size; i++) {
+                point_cloud.points[i].x = cpu_cloud[i][2];
+                point_cloud.points[i].y = -cpu_cloud[i][0];
+                point_cloud.points[i].z = -cpu_cloud[i][1];
+                point_cloud.points[i].rgb = cpu_cloud[i][3];
+            }
+
+            sensor_msgs::PointCloud2 output;
+            pcl::toROSMsg(point_cloud, output); // Convert the point cloud to a ROS message
+            output.header.frame_id = point_cloud_frame_id; // Set the header values of the ROS message
+            output.header.stamp = point_cloud_time;
+            output.height = height;
+            output.width = width;
+            output.is_bigendian = false;
+            output.is_dense = false;
+            pub_cloud.publish(output);
+        }
+
+        /* \brief Publish the informations of a camera with a ros Publisher
+         * \param cam_info_msg : the information message to publish
+         * \param pub_cam_info : the publisher object to use
+         * \param t : the ros::Time to stamp the message
+         */
+        void publishCamInfo(sensor_msgs::CameraInfoPtr cam_info_msg, ros::Publisher pub_cam_info, ros::Time t) {
+            static int seq = 0;
+            cam_info_msg->header.stamp = t;
+            cam_info_msg->header.seq = seq;
+            pub_cam_info.publish(cam_info_msg);
+            seq++;
+        }
+
+        /* \brief Get the information of the ZED cameras and store them in an information message
+         * \param zed : the sl::zed::Camera* pointer to an instance
+         * \param left_cam_info_msg : the information message to fill with the left camera informations
+         * \param right_cam_info_msg : the information message to fill with the right camera informations
+         * \param left_frame_id : the id of the reference frame of the left camera
+         * \param right_frame_id : the id of the reference frame of the right camera
+         */
+        void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr left_cam_info_msg, sensor_msgs::CameraInfoPtr right_cam_info_msg,
+                string left_frame_id, string right_frame_id, bool raw_param = false) {
+
+            int width = zed.getResolution().width;
+            int height = zed.getResolution().height;
+
+            sl::CalibrationParameters zedParam;
+
+            if (raw_param) zedParam = zed.getCameraInformation().calibration_parameters_raw;
+            else zedParam = zed.getCameraInformation().calibration_parameters;
+
+            float baseline = zedParam.T[0];
+
+            left_cam_info_msg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
+            right_cam_info_msg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
+
+            left_cam_info_msg->D.resize(5);
+            right_cam_info_msg->D.resize(5);
+            left_cam_info_msg->D[0] = zedParam.left_cam.disto[0]; // k1
+            left_cam_info_msg->D[1] = zedParam.left_cam.disto[1]; // k2
+            left_cam_info_msg->D[2] = zedParam.left_cam.disto[4]; // k3
+            left_cam_info_msg->D[3] = zedParam.left_cam.disto[2]; // p1
+            left_cam_info_msg->D[4] = zedParam.left_cam.disto[3]; // p2
+            right_cam_info_msg->D[0] = zedParam.right_cam.disto[0]; // k1
+            right_cam_info_msg->D[1] = zedParam.right_cam.disto[1]; // k2
+            right_cam_info_msg->D[2] = zedParam.right_cam.disto[4]; // k3
+            right_cam_info_msg->D[3] = zedParam.right_cam.disto[2]; // p1
+            right_cam_info_msg->D[4] = zedParam.right_cam.disto[3]; // p2
+
+            left_cam_info_msg->K.fill(0.0);
+            right_cam_info_msg->K.fill(0.0);
+            left_cam_info_msg->K[0] = zedParam.left_cam.fx;
+            left_cam_info_msg->K[2] = zedParam.left_cam.cx;
+            left_cam_info_msg->K[4] = zedParam.left_cam.fy;
+            left_cam_info_msg->K[5] = zedParam.left_cam.cy;
+            left_cam_info_msg->K[8] = right_cam_info_msg->K[8] = 1.0;
+            right_cam_info_msg->K[0] = zedParam.right_cam.fx;
+            right_cam_info_msg->K[2] = zedParam.right_cam.cx;
+            right_cam_info_msg->K[4] = zedParam.right_cam.fy;
+            right_cam_info_msg->K[5] = zedParam.right_cam.cy;
+
+            left_cam_info_msg->R.fill(0.0);
+            right_cam_info_msg->R.fill(0.0);
+            for (int i = 0; i < 3; i++) {
+                // identity
+                right_cam_info_msg->R[i + i * 3] = 1;
+                left_cam_info_msg->R[i + i * 3] = 1;
+            }
+
+            if (raw_param) {
+                cv::Mat R_ = convertRodrigues(zedParam.R);
+                float* p = (float*) R_.data;
+                for (int i = 0; i < 9; i++)
+                    right_cam_info_msg->R[i] = p[i];
+            }
+
+            left_cam_info_msg->P.fill(0.0);
+            right_cam_info_msg->P.fill(0.0);
+            left_cam_info_msg->P[0] = zedParam.left_cam.fx;
+            left_cam_info_msg->P[2] = zedParam.left_cam.cx;
+            left_cam_info_msg->P[5] = zedParam.left_cam.fy;
+            left_cam_info_msg->P[6] = zedParam.left_cam.cy;
+            left_cam_info_msg->P[10] = right_cam_info_msg->P[10] = 1.0;
+            //http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html
+            right_cam_info_msg->P[3] = (-1 * zedParam.left_cam.fx * baseline);
+
+            right_cam_info_msg->P[0] = zedParam.right_cam.fx;
+            right_cam_info_msg->P[2] = zedParam.right_cam.cx;
+            right_cam_info_msg->P[5] = zedParam.right_cam.fy;
+            right_cam_info_msg->P[6] = zedParam.right_cam.cy;
+
+            left_cam_info_msg->width = right_cam_info_msg->width = width;
+            left_cam_info_msg->height = right_cam_info_msg->height = height;
+
+            left_cam_info_msg->header.frame_id = left_frame_id;
+            right_cam_info_msg->header.frame_id = right_frame_id;
+        }
+
+        void callback(zed_wrapper::ZedConfig &config, uint32_t level) {
+            switch (level) {
+                case 0:
+                    NODELET_INFO("Reconfigure confidence : %d", config.confidence);
+                    confidence = config.confidence;
+                    break;
+                case 1:
+                    NODELET_INFO("Reconfigure exposure : %d", config.exposure);
+                    exposure = config.exposure;
+                    break;
+                case 2:
+                    NODELET_INFO("Reconfigure gain : %d", config.gain);
+                    gain = config.gain;
+                    break;
+                case 3:
+                    NODELET_INFO("Reconfigure auto control of exposure and gain : %s", config.auto_exposure ? "Enable" : "Disable");
+                    autoExposure = config.auto_exposure;
+                    if (autoExposure)
+                        triggerAutoExposure = true;
+                    break;
+            }
+        }
+
+        void device_poll() {
+            ros::Rate loop_rate(rate);
+            ros::Time old_t = ros::Time::now();
+            sl::ERROR_CODE grab_status;
+            bool tracking_activated = false;
+
+            // Get the parameters of the ZED images
+            int width = zed.getResolution().width;
+            int height = zed.getResolution().height;
+            NODELET_DEBUG_STREAM("Image size : " << width << "x" << height);
+
+            cv::Size cvSize(width, height);
+            cv::Mat leftImRGB(cvSize, CV_8UC3);
+            cv::Mat rightImRGB(cvSize, CV_8UC3);
+
+            // Create and fill the camera information messages
+            sensor_msgs::CameraInfoPtr rgb_cam_info_msg(new sensor_msgs::CameraInfo());
+            sensor_msgs::CameraInfoPtr left_cam_info_msg(new sensor_msgs::CameraInfo());
+            sensor_msgs::CameraInfoPtr right_cam_info_msg(new sensor_msgs::CameraInfo());
+            sensor_msgs::CameraInfoPtr rgb_cam_info_raw_msg(new sensor_msgs::CameraInfo());
+            sensor_msgs::CameraInfoPtr left_cam_info_raw_msg(new sensor_msgs::CameraInfo());
+            sensor_msgs::CameraInfoPtr right_cam_info_raw_msg(new sensor_msgs::CameraInfo());
+            sensor_msgs::CameraInfoPtr depth_cam_info_msg(new sensor_msgs::CameraInfo());
+            fillCamInfo(zed, left_cam_info_msg, right_cam_info_msg, left_frame_id, right_frame_id);
+            fillCamInfo(zed, left_cam_info_raw_msg, right_cam_info_raw_msg, left_frame_id, right_frame_id, true);
+            rgb_cam_info_msg = depth_cam_info_msg = left_cam_info_msg; // the reference camera is the Left one (next to the ZED logo)
+            rgb_cam_info_raw_msg = left_cam_info_raw_msg;
+
+
+            sl::RuntimeParameters runParams;
+            runParams.sensing_mode = static_cast<sl::SENSING_MODE> (sensing_mode);
+
+            sl::TrackingParameters trackParams;
+            trackParams.area_file_path = odometry_DB.c_str();
+
+
+            sl::Mat leftZEDMat, rightZEDMat, depthZEDMat;
+            // Main loop
+            while (nh_ns.ok()) {
+                // Check for subscribers
+                int rgb_SubNumber = pub_rgb.getNumSubscribers();
+                int rgb_raw_SubNumber = pub_raw_rgb.getNumSubscribers();
+                int left_SubNumber = pub_left.getNumSubscribers();
+                int left_raw_SubNumber = pub_raw_left.getNumSubscribers();
+                int right_SubNumber = pub_right.getNumSubscribers();
+                int right_raw_SubNumber = pub_raw_right.getNumSubscribers();
+                int depth_SubNumber = pub_depth.getNumSubscribers();
+                int cloud_SubNumber = pub_cloud.getNumSubscribers();
+                int odom_SubNumber = pub_odom.getNumSubscribers();
+                bool runLoop = (rgb_SubNumber + rgb_raw_SubNumber + left_SubNumber + left_raw_SubNumber + right_SubNumber + right_raw_SubNumber + depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0;
+
+                runParams.enable_point_cloud = false;
+                if (cloud_SubNumber > 0)
+                    runParams.enable_point_cloud = true;
+                // Run the loop only if there is some subscribers
+                if (runLoop) {
+                    if ((depth_stabilization || odom_SubNumber > 0) && !tracking_activated) { //Start the tracking
+                        if (odometry_DB != "" && !file_exist(odometry_DB)) {
+                            odometry_DB = "";
+                            NODELET_WARN("odometry_DB path doesn't exist or is unreachable.");
+                        }
+                        zed.enableTracking(trackParams);
+                        tracking_activated = true;
+                    } else if (!depth_stabilization && odom_SubNumber == 0 && tracking_activated) { //Stop the tracking
+                        zed.disableTracking();
+                        tracking_activated = false;
+                    }
+                    computeDepth = (depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0; // Detect if one of the subscriber need to have the depth information
+                    ros::Time t = ros::Time::now(); // Get current time
+
+                    grabbing = true;
+                    if (computeDepth) {
+                        int actual_confidence = zed.getConfidenceThreshold();
+                        if (actual_confidence != confidence)
+                            zed.setConfidenceThreshold(confidence);
+                        runParams.enable_depth = true; // Ask to compute the depth
+                    } else
+                        runParams.enable_depth = false;
+
+                    grab_status = zed.grab(runParams); // Ask to not compute the depth
+
+                    grabbing = false;
+
+                    //cout << toString(grab_status) << endl;
+                    if (grab_status != sl::ERROR_CODE::SUCCESS) { // Detect if a error occurred (for example: the zed have been disconnected) and re-initialize the ZED
+
+                        if (grab_status == sl::ERROR_CODE_NOT_A_NEW_FRAME) {
+                            NODELET_DEBUG("Wait for a new image to proceed");
+                        } else NODELET_INFO_ONCE(toString(grab_status));
+
+                        std::this_thread::sleep_for(std::chrono::milliseconds(2));
+
+                        if ((t - old_t).toSec() > 5) {
+                            zed.close();
+
+                            NODELET_INFO("Re-opening the ZED");
+                            sl::ERROR_CODE err = sl::ERROR_CODE_CAMERA_NOT_DETECTED;
+                            while (err != sl::SUCCESS) {
+                                int id = checkCameraReady(serial_number);
+                                if (id > 0) {
+                                    param.camera_linux_id = id;
+                                    err = zed.open(param); // Try to initialize the ZED
+                                    NODELET_INFO_STREAM(toString(err));
+                                } else NODELET_INFO("Waiting for the ZED to be re-connected");
+                                std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+                            }
+                            tracking_activated = false;
+                            if (depth_stabilization || odom_SubNumber > 0) { //Start the tracking
+                                if (odometry_DB != "" && !file_exist(odometry_DB)) {
+                                    odometry_DB = "";
+                                    NODELET_WARN("odometry_DB path doesn't exist or is unreachable.");
+                                }
+                                zed.enableTracking(trackParams);
+                                tracking_activated = true;
+                            }
+                        }
+                        continue;
+                    }
+
+                    old_t = ros::Time::now();
+
+                    if (autoExposure) {
+                        // getCameraSettings() can't check status of auto exposure
+                        // triggerAutoExposure is used to execute setCameraSettings() only once
+                        if (triggerAutoExposure) {
+                            zed.setCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE, 0, true);
+                            triggerAutoExposure = false;
+                        }
+                    } else {
+                        int actual_exposure = zed.getCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE);
+                        if (actual_exposure != exposure)
+                            zed.setCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE, exposure);
+
+                        int actual_gain = zed.getCameraSettings(sl::CAMERA_SETTINGS_GAIN);
+                        if (actual_gain != gain)
+                            zed.setCameraSettings(sl::CAMERA_SETTINGS_GAIN, gain);
+                    }
+
+                    // Publish the left == rgb image if someone has subscribed to
+                    if (left_SubNumber > 0 || rgb_SubNumber > 0) {
+                        // Retrieve RGBA Left image
+                        zed.retrieveImage(leftZEDMat, sl::VIEW_LEFT);
+                        cv::cvtColor(toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB);
+                        if (left_SubNumber > 0) {
+                            publishCamInfo(left_cam_info_msg, pub_left_cam_info, t);
+                            publishImage(leftImRGB, pub_left, left_frame_id, t);
+                        }
+                        if (rgb_SubNumber > 0) {
+                            publishCamInfo(rgb_cam_info_msg, pub_rgb_cam_info, t);
+                            publishImage(leftImRGB, pub_rgb, rgb_frame_id, t); // rgb is the left image
+                        }
+                    }
+
+                    // Publish the left_raw == rgb_raw image if someone has subscribed to
+                    if (left_raw_SubNumber > 0 || rgb_raw_SubNumber > 0) {
+                        // Retrieve RGBA Left image
+                        zed.retrieveImage(leftZEDMat, sl::VIEW_LEFT_UNRECTIFIED);
+                        cv::cvtColor(toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB);
+                        if (left_raw_SubNumber > 0) {
+                            publishCamInfo(left_cam_info_raw_msg, pub_left_cam_info_raw, t);
+                            publishImage(leftImRGB, pub_raw_left, left_frame_id, t);
+                        }
+                        if (rgb_raw_SubNumber > 0) {
+                            publishCamInfo(rgb_cam_info_raw_msg, pub_rgb_cam_info_raw, t);
+                            publishImage(leftImRGB, pub_raw_rgb, rgb_frame_id, t);
+                        }
+                    }
+
+                    // Publish the right image if someone has subscribed to
+                    if (right_SubNumber > 0) {
+                        // Retrieve RGBA Right image
+                        zed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT);
+                        cv::cvtColor(toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB);
+                        publishCamInfo(right_cam_info_msg, pub_right_cam_info, t);
+                        publishImage(rightImRGB, pub_right, right_frame_id, t);
+                    }
+
+                    // Publish the right image if someone has subscribed to
+                    if (right_raw_SubNumber > 0) {
+                        // Retrieve RGBA Right image
+                        zed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT_UNRECTIFIED);
+                        cv::cvtColor(toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB);
+                        publishCamInfo(right_cam_info_raw_msg, pub_right_cam_info_raw, t);
+                        publishImage(rightImRGB, pub_raw_right, right_frame_id, t);
+                    }
+
+                    // Publish the depth image if someone has subscribed to
+                    if (depth_SubNumber > 0) {
+                        zed.retrieveMeasure(depthZEDMat, sl::MEASURE_DEPTH);
+                        publishCamInfo(depth_cam_info_msg, pub_depth_cam_info, t);
+                        publishDepth(toCVMat(depthZEDMat), pub_depth, depth_frame_id, t); // in meters
+                    }
+
+                    // Publish the point cloud if someone has subscribed to
+                    if (cloud_SubNumber > 0) {
+                        // Run the point cloud conversion asynchronously to avoid slowing down all the program
+                        // Retrieve raw pointCloud data
+                        zed.retrieveMeasure(cloud, sl::MEASURE_XYZBGRA);
+                        point_cloud_frame_id = cloud_frame_id;
+                        point_cloud_time = t;
+                        publishPointCloud(width, height, pub_cloud);
+                    }
+
+                    // Transform from base to sensor
+                    tf2::Transform base_to_sensor;
+                    // Look up the transformation from base frame to camera link
+                    try {
+                        // Save the transformation from base to frame
+                        geometry_msgs::TransformStamped b2s = tfBuffer->lookupTransform(base_frame_id, camera_frame_id, t);
+                        // Get the TF2 transformation
+                        tf2::fromMsg(b2s.transform, base_to_sensor);
+
+                    } catch (tf2::TransformException &ex) {
+                        ROS_WARN_THROTTLE(10.0, "The tf from '%s' to '%s' does not seem to be available, "
+                                "will assume it as identity!",
+                                base_frame_id.c_str(),
+                                camera_frame_id.c_str());
+                        ROS_DEBUG("Transform error: %s", ex.what());
+                        base_to_sensor.setIdentity();
+                    }
+
+                    // Publish the odometry if someone has subscribed to
+                    if (odom_SubNumber > 0) {
+                        zed.getPosition(pose);
+                        // Transform ZED pose in TF2 Transformation
+                        tf2::Transform camera_transform;
+                        geometry_msgs::Transform c2s;
+                        sl::Translation translation = pose.getTranslation();
+                        c2s.translation.x = translation(2);
+                        c2s.translation.y = -translation(0);
+                        c2s.translation.z = -translation(1);
+                        sl::Orientation quat = pose.getOrientation();
+                        c2s.rotation.x = quat(2);
+                        c2s.rotation.y = -quat(0);
+                        c2s.rotation.z = -quat(1);
+                        c2s.rotation.w = quat(3);
+                        tf2::fromMsg(c2s, camera_transform);
+                        // Transformation from camera sensor to base frame
+                        base_transform = base_to_sensor * camera_transform * base_to_sensor.inverse();
+                        // Publish odometry message
+                        publishOdom(base_transform, pub_odom, odometry_frame_id, t);
+                    }
+
+                    // Publish odometry tf only if enabled
+                    if (publish_tf) {
+                        //Note, the frame is published, but its values will only change if someone has subscribed to odom
+                        publishTrackedFrame(base_transform, transform_odom_broadcaster, base_frame_id, t); //publish the tracked Frame
+                    }
+
+                    loop_rate.sleep();
+                } else {
+                    // Publish odometry tf only if enabled
+                    if (publish_tf) {
+                        publishTrackedFrame(base_transform, transform_odom_broadcaster, base_frame_id, ros::Time::now()); //publish the tracked Frame before the sleep
+                    }
+                    std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait
+                }
+            } // while loop
+            zed.close();
+        }
+
+        boost::shared_ptr<dynamic_reconfigure::Server<zed_wrapper::ZedConfig>> server;
+
+        void onInit() {
+            // Launch file parameters
+            resolution = sl::RESOLUTION_HD720;
+            quality = sl::DEPTH_MODE_PERFORMANCE;
+            sensing_mode = sl::SENSING_MODE_STANDARD;
+            rate = 30;
+            gpu_id = -1;
+            zed_id = 0;
+            serial_number = 0;
+            odometry_DB = "";
+
+            nh = getMTNodeHandle();
+            nh_ns = getMTPrivateNodeHandle();
+
+            // Set  default coordinate frames
+            // If unknown left and right frames are set in the same camera coordinate frame
+            nh_ns.param<std::string>("odometry_frame", odometry_frame_id, "odometry_frame");
+            nh_ns.param<std::string>("base_frame", base_frame_id, "base_frame");
+            nh_ns.param<std::string>("camera_frame", camera_frame_id, "camera_frame");
+            nh_ns.param<std::string>("depth_frame", depth_frame_id, "depth_frame");
+
+            // Get parameters from launch file
+            nh_ns.getParam("resolution", resolution);
+            nh_ns.getParam("quality", quality);
+            nh_ns.getParam("sensing_mode", sensing_mode);
+            nh_ns.getParam("frame_rate", rate);
+            nh_ns.getParam("odometry_DB", odometry_DB);
+            nh_ns.getParam("openni_depth_mode", openniDepthMode);
+            nh_ns.getParam("gpu_id", gpu_id);
+            nh_ns.getParam("zed_id", zed_id);
+            nh_ns.getParam("depth_stabilization", depth_stabilization);
+            int tmp_sn = 0;
+            nh_ns.getParam("serial_number", tmp_sn);
+            if (tmp_sn > 0) serial_number = tmp_sn;
+
+            // Publish odometry tf
+            nh_ns.param<bool>("publish_tf", publish_tf, true);
+
+            if (serial_number > 0)
+                ROS_INFO_STREAM("SN : " << serial_number);
+
+            // Print order frames
+            ROS_INFO_STREAM("odometry_frame: " << odometry_frame_id);
+            ROS_INFO_STREAM("base_frame: " << base_frame_id);
+            ROS_INFO_STREAM("camera_frame: " << camera_frame_id);
+            ROS_INFO_STREAM("depth_frame: " << depth_frame_id);
+            // Status of odometry TF
+            ROS_INFO_STREAM("Publish " << odometry_frame_id << " [" << (publish_tf ? "TRUE" : "FALSE") << "]");
+
+            std::string img_topic = "image_rect_color";
+            std::string img_raw_topic = "image_raw_color";
+
+            // Set the default topic names
+            string left_topic = "left/" + img_topic;
+            string left_raw_topic = "left/" + img_raw_topic;
+            string left_cam_info_topic = "left/camera_info";
+            string left_cam_info_raw_topic = "left/camera_info_raw";
+            left_frame_id = camera_frame_id;
+
+            string right_topic = "right/" + img_topic;
+            string right_raw_topic = "right/" + img_raw_topic;
+            string right_cam_info_topic = "right/camera_info";
+            string right_cam_info_raw_topic = "right/camera_info_raw";
+            right_frame_id = camera_frame_id;
+
+            string rgb_topic = "rgb/" + img_topic;
+            string rgb_raw_topic = "rgb/" + img_raw_topic;
+            string rgb_cam_info_topic = "rgb/camera_info";
+            string rgb_cam_info_raw_topic = "rgb/camera_info_raw";
+            rgb_frame_id = depth_frame_id;
+
+            string depth_topic = "depth/";
+            if (openniDepthMode) {
+                NODELET_INFO_STREAM("Openni depth mode activated");
+                depth_topic += "depth_raw_registered";
+            } else {
+                depth_topic += "depth_registered";
+            }
+
+            string depth_cam_info_topic = "depth/camera_info";
+
+            string point_cloud_topic = "point_cloud/cloud_registered";
+            cloud_frame_id = camera_frame_id;
+
+            string odometry_topic = "odom";
+
+            nh_ns.getParam("rgb_topic", rgb_topic);
+            nh_ns.getParam("rgb_raw_topic", rgb_raw_topic);
+            nh_ns.getParam("rgb_cam_info_topic", rgb_cam_info_topic);
+            nh_ns.getParam("rgb_cam_info_raw_topic", rgb_cam_info_raw_topic);
+
+            nh_ns.getParam("left_topic", left_topic);
+            nh_ns.getParam("left_raw_topic", left_raw_topic);
+            nh_ns.getParam("left_cam_info_topic", left_cam_info_topic);
+            nh_ns.getParam("left_cam_info_raw_topic", left_cam_info_raw_topic);
+
+            nh_ns.getParam("right_topic", right_topic);
+            nh_ns.getParam("right_raw_topic", right_raw_topic);
+            nh_ns.getParam("right_cam_info_topic", right_cam_info_topic);
+            nh_ns.getParam("right_cam_info_raw_topic", right_cam_info_raw_topic);
+
+            nh_ns.getParam("depth_topic", depth_topic);
+            nh_ns.getParam("depth_cam_info_topic", depth_cam_info_topic);
+
+            nh_ns.getParam("point_cloud_topic", point_cloud_topic);
+
+            nh_ns.getParam("odometry_topic", odometry_topic);
+
+            nh_ns.param<std::string>("svo_filepath", svo_filepath, std::string());
+
+
+            // Initialization transformation listener
+            tfBuffer.reset(new tf2_ros::Buffer);
+            tf_listener.reset(new tf2_ros::TransformListener(*tfBuffer));
+
+            // Initialize tf2 transformation
+            base_transform.setIdentity();
+
+            // Try to initialize the ZED
+            if (!svo_filepath.empty())
+                param.svo_input_filename = svo_filepath.c_str();
+            else {
+                param.camera_fps = rate;
+                param.camera_resolution = static_cast<sl::RESOLUTION> (resolution);
+                if (serial_number == 0)
+                    param.camera_linux_id = zed_id;
+                else {
+                    bool waiting_for_camera = true;
+                    while (waiting_for_camera) {
+                        sl::DeviceProperties prop = getZEDFromSN(serial_number);
+                        if (prop.id < -1 || prop.camera_state == sl::CAMERA_STATE::CAMERA_STATE_NOT_AVAILABLE) {
+                            std::string msg = "ZED SN" + to_string(serial_number) + " not detected ! Please connect this ZED";
+                            NODELET_INFO(msg.c_str());
+                            std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+                        } else {
+                            waiting_for_camera = false;
+                            param.camera_linux_id = prop.id;
+                        }
+                    }
+                }
+            }
+
+            param.coordinate_units = sl::UNIT_METER;
+            param.coordinate_system = sl::COORDINATE_SYSTEM_IMAGE;
+            param.depth_mode = static_cast<sl::DEPTH_MODE> (quality);
+            param.sdk_verbose = true;
+            param.sdk_gpu_id = gpu_id;
+            param.depth_stabilization = depth_stabilization;
+
+            sl::ERROR_CODE err = sl::ERROR_CODE_CAMERA_NOT_DETECTED;
+            while (err != sl::SUCCESS) {
+                err = zed.open(param);
+                NODELET_INFO_STREAM(toString(err));
+                std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+            }
+
+            serial_number = zed.getCameraInformation().serial_number;
+
+            //Reconfigure parameters
+            server = boost::make_shared<dynamic_reconfigure::Server < zed_wrapper::ZedConfig >> ();
+            dynamic_reconfigure::Server<zed_wrapper::ZedConfig>::CallbackType f;
+            f = boost::bind(&ZEDWrapperNodelet::callback, this, _1, _2);
+            server->setCallback(f);
+
+            nh_ns.getParam("confidence", confidence);
+            nh_ns.getParam("exposure", exposure);
+            nh_ns.getParam("gain", gain);
+            nh_ns.getParam("auto_exposure", autoExposure);
+            if (autoExposure)
+                triggerAutoExposure = true;
+
+            // Create all the publishers
+            // Image publishers
+            image_transport::ImageTransport it_zed(nh);
+            pub_rgb = it_zed.advertise(rgb_topic, 1); //rgb
+            NODELET_INFO_STREAM("Advertized on topic " << rgb_topic);
+            pub_raw_rgb = it_zed.advertise(rgb_raw_topic, 1); //rgb raw
+            NODELET_INFO_STREAM("Advertized on topic " << rgb_raw_topic);
+            pub_left = it_zed.advertise(left_topic, 1); //left
+            NODELET_INFO_STREAM("Advertized on topic " << left_topic);
+            pub_raw_left = it_zed.advertise(left_raw_topic, 1); //left raw
+            NODELET_INFO_STREAM("Advertized on topic " << left_raw_topic);
+            pub_right = it_zed.advertise(right_topic, 1); //right
+            NODELET_INFO_STREAM("Advertized on topic " << right_topic);
+            pub_raw_right = it_zed.advertise(right_raw_topic, 1); //right raw
+            NODELET_INFO_STREAM("Advertized on topic " << right_raw_topic);
+            pub_depth = it_zed.advertise(depth_topic, 1); //depth
+            NODELET_INFO_STREAM("Advertized on topic " << depth_topic);
+
+            //PointCloud publisher
+            pub_cloud = nh.advertise<sensor_msgs::PointCloud2> (point_cloud_topic, 1);
+            NODELET_INFO_STREAM("Advertized on topic " << point_cloud_topic);
+
+            // Camera info publishers
+            pub_rgb_cam_info = nh.advertise<sensor_msgs::CameraInfo>(rgb_cam_info_topic, 1); //rgb
+            NODELET_INFO_STREAM("Advertized on topic " << rgb_cam_info_topic);
+            pub_left_cam_info = nh.advertise<sensor_msgs::CameraInfo>(left_cam_info_topic, 1); //left
+            NODELET_INFO_STREAM("Advertized on topic " << left_cam_info_topic);
+            pub_right_cam_info = nh.advertise<sensor_msgs::CameraInfo>(right_cam_info_topic, 1); //right
+            NODELET_INFO_STREAM("Advertized on topic " << right_cam_info_topic);
+            pub_depth_cam_info = nh.advertise<sensor_msgs::CameraInfo>(depth_cam_info_topic, 1); //depth
+            NODELET_INFO_STREAM("Advertized on topic " << depth_cam_info_topic);
+            // Raw
+            pub_rgb_cam_info_raw = nh.advertise<sensor_msgs::CameraInfo>(rgb_cam_info_raw_topic, 1); // raw rgb
+            NODELET_INFO_STREAM("Advertized on topic " << rgb_cam_info_raw_topic);
+            pub_left_cam_info_raw = nh.advertise<sensor_msgs::CameraInfo>(left_cam_info_raw_topic, 1); // raw left
+            NODELET_INFO_STREAM("Advertized on topic " << left_cam_info_raw_topic);
+            pub_right_cam_info_raw = nh.advertise<sensor_msgs::CameraInfo>(right_cam_info_raw_topic, 1); // raw right
+            NODELET_INFO_STREAM("Advertized on topic " << right_cam_info_raw_topic);
+
+            //Odometry publisher
+            pub_odom = nh.advertise<nav_msgs::Odometry>(odometry_topic, 1);
+            NODELET_INFO_STREAM("Advertized on topic " << odometry_topic);
+
+            device_poll_thread = boost::shared_ptr<boost::thread> (new boost::thread(boost::bind(&ZEDWrapperNodelet::device_poll, this)));
+        }
+    }; // class ZEDROSWrapperNodelet
+} // namespace
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(zed_wrapper::ZEDWrapperNodelet, nodelet::Nodelet);

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/urdf/zed.urdf

@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2017, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<robot name="zed_camera">
+
+    <link name="zed_left_camera">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 1.57 0"/>
+            <geometry>
+                <cylinder radius="0.007" length=".031"/>
+            </geometry>
+            <material name="dark_grey">
+                <color rgba="0.1 0.1 0.1 1"/>
+            </material>
+        </visual>
+    </link>
+    <link name="zed_center">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh filename="package://zed_wrapper/urdf/ZED.stl" />
+            </geometry>
+            <material name="light_grey">
+                <color rgba="0.8 0.8 0.8 0.8"/>
+            </material>
+        </visual>
+    </link>
+    <link name="zed_right_camera">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 1.57 0"/>
+            <geometry>
+                <cylinder radius="0.007" length=".031"/>
+            </geometry>
+            <material name="dark_grey">
+                <color rgba="0.1 0.1 0.1 1"/>
+            </material>
+        </visual>
+    </link>
+    <link name="zed_depth_camera" />
+
+    <joint name="zed_left_camera_joint" type="fixed">
+        <parent link="zed_center"/>
+        <child link="zed_left_camera"/>
+        <origin xyz="0 0.06 0" rpy="0 0 0" />
+    </joint>
+    <joint name="zed_depth_camera_joint" type="fixed">
+        <parent link="zed_left_camera"/>
+        <child link="zed_depth_camera"/>
+        <origin xyz="0 0 0" rpy="-1.5707963267948966 0 -1.5707963267948966" />
+    </joint>
+
+    <joint name="zed_right_camera_joint" type="fixed">
+        <parent link="zed_center"/>
+        <child link="zed_right_camera"/>
+        <origin xyz="0 -0.06 0" rpy="0 0 0" />
+    </joint>
+
+</robot>

OSU Coursework/ROB 421 - Applied Robotics/software/ros_packages/zed_wrapper/urdf/zedm.urdf

@@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2017, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<robot name="zedm_camera">
+
+    <link name="zed_left_camera">
+
+    </link>
+    <link name="zed_center">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh filename="package://zed_wrapper/urdf/ZEDM.stl" />
+            </geometry>
+            <material name="light_grey">
+                <color rgba="0 0 0 0.9"/>
+            </material>
+        </visual>
+    </link>
+    <link name="zed_right_camera">
+
+    </link>
+    <link name="zed_depth_camera" />
+    
+    <joint name="zed_left_camera_joint" type="fixed">
+        <parent link="zed_center"/>
+        <child link="zed_left_camera"/>
+        <origin xyz="0 0.03 0" rpy="0 0 0" />
+    </joint>
+    <joint name="zed_depth_camera_joint" type="fixed">
+        <parent link="zed_left_camera"/>
+        <child link="zed_depth_camera"/>
+        <origin xyz="0 0 0" rpy="-1.5707963267948966 0 -1.5707963267948966" />
+    </joint>
+
+    <joint name="zed_right_camera_joint" type="fixed">
+        <parent link="zed_center"/>
+        <child link="zed_right_camera"/>
+        <origin xyz="0 -0.03 0" rpy="0 0 0" />
+    </joint>
+ 
+</robot>

OSU Coursework/ROB 421 - Applied Robotics/software/windows_packages/autohotkey_scripts/enable_robot.ahk

@@ -0,0 +1,87 @@
+#NoEnv  ; Recommended for performance and compatibility with future AutoHotkey releases.
+; #Warn  ; Enable warnings to assist with detecting common errors.
+SendMode Input  ; Recommended for new scripts due to its superior speed and reliability.
+SetWorkingDir %A_ScriptDir%  ; Ensures a consistent starting directory.
+
+connect_status_box_x := 18
+connect_status_box_y := 86
+connect_status_green := "0x00FF00"
+
+run_status_box_x := 13
+run_status_box_y := 106
+run_status_green := "0x00FF00"
+
+error_status_box_x := 45
+error_status_box_y := 431
+error_status_grey := "0xFFFFFF"
+
+connect_button_x := 73
+connect_button_y := 88
+
+run_button_x := 72
+run_button_y := 106
+
+clear_button_x := 88
+clear_button_y := 446
+
+should_exit = 0
+
+Loop 
+{
+	If (should_exit)
+	{
+	ExitApp
+	}
+	
+	IfWinNotExist, RobMaster
+	{
+		Run, C:\ORiN2\CAO\ProviderLib\DENSO\NetwoRC\Bin\RobMaster.exe
+		Sleep, 3000
+	}
+	
+	IfWinExist, RobMaster
+	{
+		WinActivate
+	}
+	
+	IfWinActive, RobMaster
+	{
+		PixelGetColor, connect_status_color, %connect_status_box_x%, %connect_status_box_y%
+		;MsgBox, Color is %connect_status_color%, other color is %connect_status_green% 
+		
+		If( connect_status_color <> connect_status_green )
+		{
+			Click, %connect_button_x%, %connect_button_y%
+			Sleep, 250
+		}
+		
+		PixelGetColor, error_status_color, %error_status_box_x%, %error_status_box_y%
+		
+		If( error_status_color <> error_status_grey )
+		{
+			Click, %clear_button_x%, %clear_button_y%
+			Sleep, 250
+		}
+		else
+		{
+			PixelGetColor, run_status_color, %run_status_box_x%, %run_status_box_y%
+			
+			If( run_status_color <> run_status_green )
+			{
+				Click, %run_button_x%, %run_button_y%
+				Sleep, 250
+			
+			}
+		
+		}
+	}
+	
+	Sleep, 100
+}
+
+^q::
+should_exit = 1
+return
+
+Return
+

OSU Coursework/ROB 421 - Applied Robotics/software/windows_packages/denso_slave/Framework/arm_control_receiver.py

@@ -0,0 +1,260 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import pythoncom
+import win32com.client
+from time import time
+import socket
+import json
+
+#####################################
+# Global Variables
+#####################################
+THREAD_HERTZ = 100
+
+P0 = (216.1302490234375, -9.575998306274414, 572.6145629882812, 63.89561462402344, 8.09478759765625, 83.43250274658203)
+P1 = (251.22869873046875, -9.575998306274414, 572.6145629882812, 63.89561462402344, 8.09478759765625, 83.43250274658203)
+P2 = (216.1302490234375, 0.10808953642845154, 606.7885131835938, 63.89561462402344, 8.09478759765625, 83.43250274658203)
+
+J0 = (-2.4949951171875, -68.55029296875, 161.4649658203125, 0.2345581203699112, -40.739683151245117, 60.7391586303711)
+
+BAD_VAL = -1000000
+
+TCP_PORT = 9877
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class RAWControlReceiver(QtCore.QThread):
+
+    new_message__signal = QtCore.pyqtSignal(dict)
+
+    def __init__(self):
+        super(RAWControlReceiver, self).__init__()
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+        self.wait_time = 1.0 / THREAD_HERTZ
+
+        self.control_tcp_server = None
+        self.client_connection = None
+        self.client_address = None
+
+        self.current_message = ""
+
+        self.num_messages = 0
+        self.last_time = time()
+
+    def run(self):
+        self.initialize_tcp_server()
+        while self.run_thread_flag:
+            self.check_for_new_command_message()
+            # self.msleep(2)
+
+    def initialize_tcp_server(self):
+        self.control_tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.control_tcp_server.bind(('', TCP_PORT))
+        self.control_tcp_server.listen(5)
+
+    def check_for_new_command_message(self):
+        try:
+            self.current_message += self.client_connection.recv(8)
+
+            found_pound = self.current_message.find("#####")
+
+            if found_pound != -1:
+                split_message = str(self.current_message[:found_pound])
+
+                self.current_message = self.current_message[found_pound + 5:]
+
+                try:
+                    json_message = json.loads(split_message)
+
+                    self.num_messages += 1
+
+                    if time() - self.last_time > 1:
+                        print "Num commands received:", self.num_messages
+                        self.num_messages = 0
+                        self.last_time = time()
+
+                    self.new_message__signal.emit(json_message)
+                except Exception, e:
+                    print e, "could not parse"
+        except Exception, e:
+            print e, "other"
+            self.client_connection, self.client_address = self.control_tcp_server.accept()
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class ArmControlReceiver(QtCore.QThread):
+    def __init__(self, shared_objects):
+        super(ArmControlReceiver, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.status_sender_class = self.shared_objects["threaded_classes"]["Arm Status Sender"]
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Get the settings instance ##########
+        self.message_handler = RAWControlReceiver()
+        self.message_handler.start()
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+        self.wait_time = 1.0 / THREAD_HERTZ
+
+        self.control_tcp_server = None
+        self.client_connection = None
+        self.client_address = None
+
+        self.cao_engine = None
+        self.controller = None
+        self.arm = None
+
+        self.CONTROL_COMMANDS = {
+            "enable_motors": self.enable_motors,
+            "change_robot_speed": self.change_robot_speed,
+            "move_position_abs": self.move_arm_position_absolute,
+            "move_position_rel": self.move_arm_position_relative,
+            "move_joint_abs": self.move_joints_absolute,
+            "move_joint_rel": self.move_joints_relative,
+
+            "charge_tank_psi": 0,
+            "fire_tank": 0
+        }
+
+        self.current_message = ""
+
+        self.command_queue = []
+
+        self.num_commands = 0
+
+        self.last_commands_time = time()
+
+    def run(self):
+        self.initialize_cao_engine()
+
+        while self.run_thread_flag:
+            start_time = time()
+
+            # self.add_item_to_command_queue({"move_joint_rel": (10, 0, 0, 0, 0, 0)})
+            # self.add_item_to_command_queue({"move_joint_rel": (-10, 0, 0, 0, 0, 0)})
+
+            self.process_command_queue_item()
+
+            if time() - self.last_commands_time > 1:
+                print "Num commands processed:", self.num_commands
+                self.num_commands = 0
+                self.last_commands_time = time()
+
+            time_diff = time() - start_time
+            self.msleep(max(int(self.wait_time - time_diff), 0))
+
+    def initialize_cao_engine(self):
+        pythoncom.CoInitialize()
+        self.cao_engine = win32com.client.Dispatch("CAO.CaoEngine")
+        self.controller = self.cao_engine.Workspaces(0).AddController("RC", "CaoProv.DENSO.NetwoRC", "", "conn=eth:192.168.1.10")
+        self.arm = self.controller.AddRobot("Arm1", "")
+
+    def on_new_message__signal(self, message):
+        self.command_queue.append(message)
+
+    def process_command_queue_item(self):
+        if self.command_queue:
+            key = list(self.command_queue[0].keys())[0]
+            data = self.command_queue[0][key]
+
+            del self.command_queue[0]
+
+            command_to_run = self.CONTROL_COMMANDS.get(key)
+            command_to_run(data)
+
+            self.num_commands += 1
+
+    def add_item_to_command_queue(self, item):
+        self.command_queue.append(item)
+
+    def enable_motors(self, should_enable):
+        try:
+            if should_enable:
+                self.arm.Execute("Motor", 1)
+                self.arm.Execute("TakeArm", 0)
+            else:
+                self.arm.Execute("Motor", 0)
+                self.arm.Execute("GiveArm", 0)
+        except:
+            print("Arm not able to change to state", "on." if should_enable else "off.")
+
+    def change_robot_speed(self, speed):
+        self.arm.Execute("ExtSpeed", (speed, speed, speed))
+
+    def move_arm_position_absolute(self, position):
+        try:
+            if self.status_sender_class.statuses["motor_enabled"]:
+                self.arm.Move(1, "@P " + str(tuple(position)), "NEXT")
+        except:
+            pass
+
+    def move_arm_position_relative(self, position_offsets):
+        current_position = self.status_sender_class.position
+
+        if current_position["rz"] == BAD_VAL or len(position_offsets) == position_offsets.count(0):
+            return
+
+        new_position = (
+            current_position["x"] + position_offsets[0],
+            current_position["y"] + position_offsets[1],
+            current_position["z"] + position_offsets[2],
+            current_position["rx"] + position_offsets[3],
+            current_position["ry"] + position_offsets[4],
+            current_position["rz"] + position_offsets[5],
+        )
+        # print "here"
+        self.move_arm_position_absolute(new_position)
+
+    def move_joints_absolute(self, joint_positions):
+        try:
+            if self.status_sender_class.statuses["motor_enabled"]:
+                self.arm.Move(1, "J" + str(tuple(joint_positions)), "NEXT")
+        except:
+            pass
+
+    def move_joints_relative(self, joint_position_offsets):
+        current_position = self.status_sender_class.joints
+
+        if current_position[6] == BAD_VAL or len(joint_position_offsets) == joint_position_offsets.count(0):
+            return
+
+        new_joint_positions = (
+            current_position[1] + joint_position_offsets[0],
+            current_position[2] + joint_position_offsets[1],
+            current_position[3] + joint_position_offsets[2],
+            current_position[4] + joint_position_offsets[3],
+            current_position[5] + joint_position_offsets[4],
+            current_position[6] + joint_position_offsets[5],
+        )
+
+        self.move_joints_absolute(new_joint_positions)
+
+    def connect_signals_and_slots(self):
+        self.message_handler.new_message__signal.connect(self.on_new_message__signal)
+
+    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.message_handler.run_thread_flag = False
+        self.message_handler.wait()
+        self.run_thread_flag = False

OSU Coursework/ROB 421 - Applied Robotics/software/windows_packages/denso_slave/Framework/arm_status_sender.py

@@ -0,0 +1,208 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import pythoncom
+import win32com.client
+import time
+from time import time
+import json
+import socket
+
+#####################################
+# Global Variables
+#####################################
+THREAD_HERTZ = 60
+
+MESSAGE_HERTZ = 5
+
+TCP_PORT = 9876
+
+BAD_VAL = -1000000
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class NetworkSender(QtCore.QThread):
+    def __init__(self):
+        pass
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class ArmStatusSender(QtCore.QThread):
+
+    position = {
+        "x": BAD_VAL,
+        "y": BAD_VAL,
+        "z": BAD_VAL,
+        "rx": BAD_VAL,
+        "ry": BAD_VAL,
+        "rz": BAD_VAL,
+        "fig": BAD_VAL
+    }
+
+    joints = {
+        1: BAD_VAL,
+        2: BAD_VAL,
+        3: BAD_VAL,
+        4: BAD_VAL,
+        5: BAD_VAL,
+        6: BAD_VAL
+    }
+
+    statuses = {
+        "motor_enabled": True,
+        "arm_normal": False,
+        "error": "",
+
+        "speed": 0,
+        "arm_busy": True,
+
+        "tank_psi": 0
+    }
+
+    def __init__(self, shared_objects):
+        super(ArmStatusSender, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+        self.wait_time = 1.0 / THREAD_HERTZ
+
+        self.status_tcp_server = None
+        self.client_connection = None
+        self.client_address = None
+
+        self.cao_engine = None
+        self.controller = None
+        self.arm = None
+
+        self.robot_enabled = None
+        self.robot_normal = None
+        self.robot_error = None
+        self.robot_busy = None
+        self.robot_speed = None
+
+        self.robot_current_position = None
+        self.robot_current_joint_angles = None
+
+        self.last_packet_sent_time = time()
+
+    def run(self):
+        self.initialize_cao_engine_and_watchers()
+        self.initialize_tcp_server()
+
+        while self.run_thread_flag:
+            start_time = time()
+
+            self.get_statuses()
+            self.get_position()
+            self.get_joint_angles()
+
+            message_diff = time() - self.last_packet_sent_time
+
+            # TODO: SPLIT THIS OUT IF IT CAUSES PROBLEMS!!!!!!
+            if message_diff > (1.0 / MESSAGE_HERTZ):
+                self.send_status_package()
+                self.last_packet_sent_time = time()
+
+            time_diff = time() - start_time
+            self.msleep(max(int(self.wait_time - time_diff) * 1000, 0))
+
+    def initialize_cao_engine_and_watchers(self):
+        pythoncom.CoInitialize()
+        self.cao_engine = win32com.client.Dispatch("CAO.CaoEngine")
+        self.controller = self.cao_engine.Workspaces(0).AddController("RC", "CaoProv.DENSO.NetwoRC", "", "conn=eth:192.168.1.10")
+        self.arm = self.controller.AddRobot("Arm1", "")
+
+        self.robot_enabled = self.arm.AddVariable("@SERVO_ON", "")
+        self.robot_normal = self.controller.AddVariable("@NORMAL_STATUS", "")
+        self.robot_error = self.controller.AddVariable("@ERROR_DESCRIPTION", "")
+        self.robot_busy = self.arm.AddVariable("@BUSY_STATUS", "")
+
+        self.robot_speed = self.arm.AddVariable("@EXTSPEED", "")
+        self.robot_current_position = self.arm.AddVariable("@CURRENT_POSITION", "")
+        self.robot_current_joint_angles = self.arm.AddVariable("@CURRENT_ANGLE", "")
+
+    def initialize_tcp_server(self):
+        self.status_tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.status_tcp_server.bind(('', TCP_PORT))
+        self.status_tcp_server.listen(5)
+
+    def get_statuses(self):
+        motor_on = self.robot_enabled.Value
+        normal = self.robot_normal.Value
+        error = self.robot_error.Value
+        busy = self.robot_busy.Value
+        robot_speed = self.robot_speed.Value
+
+        self.statuses = {
+            "motor_enabled": motor_on,
+            "arm_normal": normal,
+            "error": error,
+
+            "speed": robot_speed,
+            "arm_busy": busy,
+
+            "tank_psi": 0
+        }
+
+    def get_position(self):
+        x, y, z, rx, ry, rz, fig = self.robot_current_position.Value
+        self.position = {
+            "x": x,
+            "y": y,
+            "z": z,
+            "rx": rx,
+            "ry": ry,
+            "rz": rz,
+            "fig": fig
+        }
+
+    def send_status_package(self):
+        package = {
+            "statuses": self.statuses,
+            "position": self.position,
+            "joints": self.joints
+        }
+
+        try:
+            self.client_connection.sendall(json.dumps(package))
+            self.client_connection.sendall("#####")
+        except:
+            self.client_connection, self.client_address = self.status_tcp_server.accept()
+
+    def get_joint_angles(self):
+        j1, j2, j3, j4, j5, j6, _, _ = self.robot_current_joint_angles.Value
+
+        self.joints = {
+            1: j1,
+            2: j2,
+            3: j3,
+            4: j4,
+            5: j5,
+            6: j6
+        }
+        # print(self.robot_current_joint_angles.Value)
+
+    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

OSU Coursework/ROB 421 - Applied Robotics/software/windows_packages/denso_slave/Framework/jsonsocket.py

@@ -0,0 +1,134 @@
+import json, socket
+
+class Server(object):
+  """
+  A JSON socket server used to communicate with a JSON socket client. All the
+  data is serialized in JSON. How to use it:
+
+  server = Server(host, port)
+  while True:
+    server.accept()
+    data = server.recv()
+    # shortcut: data = server.accept().recv()
+    server.send({'status': 'ok'})
+  """
+
+  backlog = 5
+  client = None
+
+  def __init__(self, host, port):
+    self.socket = socket.socket()
+    self.socket.bind((host, port))
+    self.socket.listen(self.backlog)
+
+  def __del__(self):
+    self.close()
+
+  def accept(self):
+    # if a client is already connected, disconnect it
+    if self.client:
+      self.client.close()
+    self.client, self.client_addr = self.socket.accept()
+    return self
+
+  def send(self, data):
+    if not self.client:
+      raise Exception('Cannot send data, no client is connected')
+    _send(self.client, data)
+    return self
+
+  def recv(self):
+    if not self.client:
+      raise Exception('Cannot receive data, no client is connected')
+    return _recv(self.client)
+
+  def close(self):
+    if self.client:
+      self.client.close()
+      self.client = None
+    if self.socket:
+      self.socket.close()
+      self.socket = None
+
+
+class Client(object):
+  """
+  A JSON socket client used to communicate with a JSON socket server. All the
+  data is serialized in JSON. How to use it:
+
+  data = {
+    'name': 'Patrick Jane',
+    'age': 45,
+    'children': ['Susie', 'Mike', 'Philip']
+  }
+  client = Client()
+  client.connect(host, port)
+  client.send(data)
+  response = client.recv()
+  # or in one line:
+  response = Client().connect(host, port).send(data).recv()
+  """
+
+  socket = None
+
+  def __del__(self):
+    self.close()
+
+  def connect(self, host, port):
+    self.socket = socket.socket()
+    self.socket.connect((host, port))
+    return self
+
+  def send(self, data):
+    if not self.socket:
+      raise Exception('You have to connect first before sending data')
+    _send(self.socket, data)
+    return self
+
+  def recv(self):
+    if not self.socket:
+      raise Exception('You have to connect first before receiving data')
+    return _recv(self.socket)
+
+  def recv_and_close(self):
+    data = self.recv()
+    self.close()
+    return data
+
+  def close(self):
+    if self.socket:
+      self.socket.close()
+      self.socket = None
+
+
+## helper functions ##
+
+def _send(socket, data):
+  try:
+    serialized = json.dumps(data)
+  except (TypeError, ValueError), e:
+    raise Exception('You can only send JSON-serializable data')
+  # send the length of the serialized data first
+  socket.send('%d\n' % len(serialized))
+  # send the serialized data
+  socket.sendall(serialized)
+
+def _recv(socket):
+  # read the length of the data, letter by letter until we reach EOL
+  length_str = ''
+  char = socket.recv(1)
+  while char != '\n':
+    length_str += char
+    char = socket.recv(1)
+  total = int(length_str)
+  # use a memoryview to receive the data chunk by chunk efficiently
+  view = memoryview(bytearray(total))
+  next_offset = 0
+  while total - next_offset > 0:
+    recv_size = socket.recv_into(view[next_offset:], total - next_offset)
+    next_offset += recv_size
+  try:
+    deserialized = json.loads(view.tobytes())
+  except (TypeError, ValueError), e:
+    raise Exception('Data received was not in JSON format')
+  return deserialized

OSU Coursework/ROB 421 - Applied Robotics/software/windows_packages/denso_slave/Framework/socket_test.py

@@ -0,0 +1,88 @@
+# import time
+# from socket import socket, gethostbyname, AF_INET, SOCK_DGRAM
+#
+# SERVER_IP = '192.168.1.14'
+# PORT_NUMBER = 5000
+#
+# mySocket = socket( AF_INET, SOCK_DGRAM )
+# mySocket.connect((SERVER_IP,PORT_NUMBER))
+#
+# while True:
+#         mySocket.send('cool')
+#         time.sleep(.5)
+
+# from socket import socket, gethostbyname, AF_INET, SOCK_DGRAM
+# import sys
+# PORT_NUMBER = 5000
+# SIZE = 1024
+#
+# hostName = gethostbyname( '0.0.0.0' )
+#
+# mySocket = socket( AF_INET, SOCK_DGRAM )
+# mySocket.bind( (hostName, PORT_NUMBER) )
+#
+# print ("Test server listening on port {0}\n".format(PORT_NUMBER))
+#
+# while True:
+#         (data,addr) = mySocket.recvfrom(SIZE)
+#         print data
+
+# import jsocket
+# import logging
+# import time
+#
+# logger = logging.getLogger("jsocket.example_servers")
+#
+# class MyFactoryThread(jsocket.ServerFactoryThread):
+#     def __init__(self):
+#         super(MyFactoryThread, self).__init__()
+#         self.address = "0.0.0.0"
+#         self.port = 21151
+#
+#     ##
+#     #  virtual method - Implementer must define protocol
+#     def _process_message(self, obj):
+#         if obj != '':
+#             if obj['message'] == "new connection":
+#                 logger.info("new connection.")
+#             else:
+#                 logger.info(obj)
+#
+#
+# if __name__ == "__main__":
+#     server = jsocket.ServerFactory(MyFactoryThread)
+#     server.timeout = 2.0
+#     server.start()
+#
+#     while True:
+#         time.sleep(0.1)
+
+import socket
+
+HOST = ''
+PORT = 9876
+ADDR = (HOST,PORT)
+BUFSIZE = 4096
+
+serv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+serv.bind(ADDR)
+serv.listen(5)
+
+print 'listening ...'
+
+while True:
+  conn, addr = serv.accept()
+  print 'client connected ... ', addr
+  myfile = open('testfile.mov', 'w')
+
+  while True:
+    data = conn.recv(BUFSIZE)
+    if not data: break
+    print data
+    print 'writing file ....'
+
+  myfile.close()
+  print 'finished writing file'
+  conn.close()
+  print 'client disconnected'

OSU Coursework/ROB 421 - Applied Robotics/software/windows_packages/denso_slave/denso_slave.py

@@ -0,0 +1,73 @@
+import sys
+from PyQt5 import QtWidgets, QtCore, QtGui, uic
+import signal
+
+from Framework.arm_status_sender import ArmStatusSender as ArmStatusSender
+from Framework.arm_control_receiver import ArmControlReceiver as ArmControlReceiver
+
+
+#####################################
+# GroundStation Class Definition
+#####################################
+class NetworkSlave(QtCore.QObject):
+    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(NetworkSlave, self).__init__(parent)
+
+        # ########## Get the Pick And Plate instance of the logger ##########
+        # self.logger = logging.getLogger("groundstation")
+
+        self.shared_objects = {
+            "regular_classes": {},
+            "threaded_classes": {}
+        }
+
+        # ##### Instantiate Regular Classes ######
+
+        # ##### Instantiate Threaded Classes ######
+        self.__add_thread("Arm Status Sender", ArmStatusSender(self.shared_objects))
+        self.__add_thread("Arm Control Receiver", ArmControlReceiver(self.shared_objects))
+
+        self.connect_signals_and_slots_signal.emit()
+        self.__connect_signals_to_slots()
+        self.start_threads_signal.emit()
+
+    def __add_thread(self, thread_name, instance):
+        self.shared_objects["threaded_classes"][thread_name] = instance
+        instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal,
+                               self.kill_threads_signal)
+
+    def __connect_signals_to_slots(self):
+        pass
+
+    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()
+
+        QtCore.QCoreApplication.exit()
+
+
+#####################################
+# 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 = QtCore.QCoreApplication(sys.argv)  # Create the ase qt gui application
+
+    # ########## Set Organization Details for QSettings ##########
+    QtCore.QCoreApplication.setOrganizationName("OSURC")
+    QtCore.QCoreApplication.setOrganizationDomain("http://osurobotics.club/")
+    QtCore.QCoreApplication.setApplicationName("denso_slave")
+
+    # ########## Start Ground Station If Ready ##########
+    network_slave = NetworkSlave()
+    application.exec_()  # Execute launching of the application

OSU Coursework/ROB 421 - Applied Robotics/tools/pendant/Framework/cao_controller.py

@@ -0,0 +1,195 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import pythoncom
+import win32com.client
+import time
+from time import time
+
+#####################################
+# Global Variables
+#####################################
+THREAD_HERTZ = 100
+
+P0 = (216.1302490234375, -9.575998306274414, 572.6145629882812, 63.89561462402344, 8.09478759765625, 83.43250274658203)
+P0_2 = (220.1302490234375, -9.575998306274414, 572.6145629882812, 63.89561462402344, 8.09478759765625, 83.43250274658203)
+P1 = (251.22869873046875, -9.575998306274414, 572.6145629882812, 63.89561462402344, 8.09478759765625, 83.43250274658203)
+P2 = (216.1302490234375, 0.10808953642845154, 606.7885131835938, 63.89561462402344, 8.09478759765625, 83.43250274658203)
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class CAOController(QtCore.QThread):
+    j1_set__signal = QtCore.pyqtSignal(float)
+    j2_set__signal = QtCore.pyqtSignal(float)
+    j3_set__signal = QtCore.pyqtSignal(float)
+    j4_set__signal = QtCore.pyqtSignal(float)
+    j5_set__signal = QtCore.pyqtSignal(float)
+    j6_set__signal = QtCore.pyqtSignal(float)
+
+    def __init__(self, shared_objects):
+        super(CAOController, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.main_screen = self.shared_objects["screens"]["main_screen"]
+        self.motor_power_label = self.main_screen.motor_power_label  # type: QtWidgets.QLabel
+
+        self.control_mode_pushbutton = self.main_screen.control_mode_stored_points_radiobutton  # type: QtWidgets.QRadioButton
+        self.control_mode_pushbutton.setChecked(True)
+
+        self.move_p0_pushbutton = self.main_screen.movep0_pushbutton  # type: QtWidgets.QPushButton
+        self.move_p1_pushbutton = self.main_screen.movep1_pushbutton  # type: QtWidgets.QPushButton
+        self.move_p2_pushbutton = self.main_screen.movep2_pushbutton  # type: QtWidgets.QPushButton
+
+        self.max_speed_label = self.main_screen.max_speed_label  # type: QtWidgets.QLabel
+        self.speed_limit_spinbox = self.main_screen.speed_limit_spinbox  # type: QtWidgets.QSpinBox
+        self.speed_limit_apply_pushbutton = self.main_screen.speed_limit_apply_pushbutton  # type: QtWidgets.QPushButton
+
+        self.j1_spinbox = self.main_screen.j1_spinbox  # type: QtWidgets.QSpinBox
+        self.j2_spinbox = self.main_screen.j2_spinbox  # type: QtWidgets.QSpinBox
+        self.j3_spinbox = self.main_screen.j3_spinbox  # type: QtWidgets.QSpinBox
+        self.j4_spinbox = self.main_screen.j4_spinbox  # type: QtWidgets.QSpinBox
+        self.j5_spinbox = self.main_screen.j5_spinbox  # type: QtWidgets.QSpinBox
+        self.j6_spinbox = self.main_screen.j6_spinbox  # type: QtWidgets.QSpinBox
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+        self.wait_time = 1.0 / THREAD_HERTZ
+
+        self.cao_engine = None
+        self.controller = None
+        self.arm = None
+
+        self.arm_power_status = None
+
+        self.command_queue = []
+
+        self.enable_motors = False
+
+        self.set_motor_speed = True
+        self.new_motor_speed = 5
+
+        self.move_p0 = False
+        self.move_p1 = False
+        self.move_p2 = False
+
+        self.arm_last_power_status = -1
+
+    def run(self):
+        pythoncom.CoInitialize()
+        self.cao_engine = win32com.client.Dispatch("CAO.CaoEngine")
+        self.controller = self.cao_engine.Workspaces(0).AddController("RC", "CaoProv.DENSO.NetwoRC", "", "conn=eth:192.168.1.10")
+        self.arm = self.controller.AddRobot("Arm1", "")
+
+        # p_val_0 = self.controller.AddVariable("P0", "")
+        self.arm_power_status = self.arm.AddVariable("@SERVO_ON", "")
+        self.arm_speed_status = self.arm.AddVariable("@EXTSPEED", "")
+        self.arm_current_position = self.arm.AddVariable("@CURRENT_POSITION", "")
+        # self.arm_current_joint_states = self.arm.AddVariable("@CURJNT", "")
+
+        temp = 1
+
+        while self.run_thread_flag:
+
+            start_time = time()
+            # print(self.arm_current_joint_states.Value)
+            test = list(self.arm_current_position.Value)
+            #
+            # self.j1_set__signal.emit(j1)
+            # self.j2_set__signal.emit(j2)
+            # self.j3_set__signal.emit(j3)
+            # self.j4_set__signal.emit(j4)
+            # self.j5_set__signal.emit(j5)
+            # self.j6_set__signal.emit(j6)
+
+            self.max_speed_label.setText(str(self.arm_speed_status.Value))
+
+            if self.arm_power_status.Value != self.arm_last_power_status:
+                if self.arm_power_status.Value:
+                    self.motor_power_label.setText("MOTOR POWER ON")
+                    self.motor_power_label.setStyleSheet("background-color: green;")
+                else:
+                    self.motor_power_label.setText("MOTOR POWER OFF")
+                    self.motor_power_label.setStyleSheet("background-color: darkred;")
+
+                self.arm_last_power_status = self.arm_power_status.Value
+
+            if self.enable_motors:
+                if not self.arm_power_status.Value:
+                    self.arm.Execute("Motor", 1)  # Enable motor power
+                    self.arm.Execute("TakeArm", 0)
+                else:
+                    self.arm.Execute("Motor", 0)  # Enable motor power
+
+                self.enable_motors = False
+
+            if self.set_motor_speed:
+                self.arm.Execute("ExtSpeed", (self.new_motor_speed, self.new_motor_speed, self.new_motor_speed))
+                self.set_motor_speed = False
+
+            if self.move_p0:
+                test[0] += 10
+                print(test)
+                self.arm.Move(1, str(tuple(test)), "")
+                self.move_p0 = False
+
+            if self.move_p1:
+                self.arm.Move(1, str(P0_2), "")
+                self.move_p1 = False
+
+            if self.move_p2:
+                self.arm.Move(1, str(P2), "")
+                self.move_p2 = False
+
+            time_diff = time() - start_time
+
+            self.msleep(max(int(self.wait_time - time_diff), 0))
+
+    def toggle_motor_power__slot(self, _):
+        self.enable_motors = True
+
+    def set_motor_speed__slot(self):
+        self.set_motor_speed = True
+        self.new_motor_speed = self.speed_limit_spinbox.value()
+
+    def move_p0__slot(self):
+        self.move_p0 = True
+
+    def move_p1__slot(self):
+        self.move_p1 = True
+
+    def move_p2__slot(self):
+        self.move_p2 = True
+
+    def connect_signals_and_slots(self):
+        self.motor_power_label.mousePressEvent = self.toggle_motor_power__slot
+
+        self.speed_limit_apply_pushbutton.clicked.connect(self.set_motor_speed__slot)
+
+        self.move_p0_pushbutton.clicked.connect(self.move_p0__slot)
+        self.move_p1_pushbutton.clicked.connect(self.move_p1__slot)
+        self.move_p2_pushbutton.clicked.connect(self.move_p2__slot)
+
+        self.j1_set__signal.connect(self.j1_spinbox.setValue)
+        self.j2_set__signal.connect(self.j2_spinbox.setValue)
+        self.j3_set__signal.connect(self.j3_spinbox.setValue)
+        self.j4_set__signal.connect(self.j4_spinbox.setValue)
+        self.j5_set__signal.connect(self.j5_spinbox.setValue)
+        self.j6_set__signal.connect(self.j6_spinbox.setValue)
+
+    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

OSU Coursework/ROB 421 - Applied Robotics/tools/pendant/Framework/cao_monitor.py

@@ -0,0 +1,102 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import pythoncom
+import win32com.client
+import time
+from time import time
+
+#####################################
+# Global Variables
+#####################################
+THREAD_HERTZ = 100
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class CAOMonitor(QtCore.QThread):
+    j1_set__signal = QtCore.pyqtSignal(float)
+    j2_set__signal = QtCore.pyqtSignal(float)
+    j3_set__signal = QtCore.pyqtSignal(float)
+    j4_set__signal = QtCore.pyqtSignal(float)
+    j5_set__signal = QtCore.pyqtSignal(float)
+    j6_set__signal = QtCore.pyqtSignal(float)
+
+    def __init__(self, shared_objects):
+        super(CAOMonitor, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.main_screen = self.shared_objects["screens"]["main_screen"]
+        self.motor_power_label = self.main_screen.motor_power_label  # type: QtWidgets.QLabel
+
+        self.max_speed_label = self.main_screen.max_speed_label  # type: QtWidgets.QLabel\
+
+        self.j1_spinbox = self.main_screen.j1_spinbox  # type: QtWidgets.QSpinBox
+        self.j2_spinbox = self.main_screen.j2_spinbox  # type: QtWidgets.QSpinBox
+        self.j3_spinbox = self.main_screen.j3_spinbox  # type: QtWidgets.QSpinBox
+        self.j4_spinbox = self.main_screen.j4_spinbox  # type: QtWidgets.QSpinBox
+        self.j5_spinbox = self.main_screen.j5_spinbox  # type: QtWidgets.QSpinBox
+        self.j6_spinbox = self.main_screen.j6_spinbox  # type: QtWidgets.QSpinBox
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+        self.wait_time = 1.0 / THREAD_HERTZ
+
+        self.cao_engine = None
+        self.controller = None
+        self.arm = None
+
+        self.arm_power_status = None
+
+    def run(self):
+        pythoncom.CoInitialize()
+        self.cao_engine = win32com.client.Dispatch("CAO.CaoEngine")
+        self.controller = self.cao_engine.Workspaces(0).AddController("RC", "CaoProv.DENSO.NetwoRC", "", "conn=eth:192.168.1.10")
+        self.arm = self.controller.AddRobot("Arm1", "")
+
+        self.arm_power_status = self.arm.AddVariable("@SERVO_ON", "")
+        self.arm_speed_status = self.arm.AddVariable("@EXTSPEED", "")
+        self.arm_current_position = self.arm.AddVariable("@CURRENT_POSITION", "")
+
+        while self.run_thread_flag:
+
+            start_time = time()
+            j1, j2, j3, j4, j5, j6, _ = self.arm_current_position.Value
+
+            self.j1_set__signal.emit(j1)
+            self.j2_set__signal.emit(j2)
+            self.j3_set__signal.emit(j3)
+            self.j4_set__signal.emit(j4)
+            self.j5_set__signal.emit(j5)
+            self.j6_set__signal.emit(j6)
+
+            self.max_speed_label.setText(str(self.arm_speed_status.Value))
+
+            time_diff = time() - start_time
+
+            self.msleep(max(int(self.wait_time - time_diff), 0))
+
+    def connect_signals_and_slots(self):
+        self.j1_set__signal.connect(self.j1_spinbox.setValue)
+        self.j2_set__signal.connect(self.j2_spinbox.setValue)
+        self.j3_set__signal.connect(self.j3_spinbox.setValue)
+        self.j4_set__signal.connect(self.j4_spinbox.setValue)
+        self.j5_set__signal.connect(self.j5_spinbox.setValue)
+        self.j6_set__signal.connect(self.j6_spinbox.setValue)
+
+    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

OSU Coursework/ROB 421 - Applied Robotics/tools/pendant/Resources/Ui/pendant.ui

@@ -0,0 +1,382 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>325</width>
+    <height>542</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <layout class="QGridLayout" name="gridLayout">
+    <item row="1" column="0">
+     <layout class="QVBoxLayout" name="verticalLayout">
+      <item>
+       <widget class="QLabel" name="label_8">
+        <property name="font">
+         <font>
+          <pointsize>12</pointsize>
+          <weight>75</weight>
+          <bold>true</bold>
+         </font>
+        </property>
+        <property name="text">
+         <string>Axis Readout/Control</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <layout class="QFormLayout" name="formLayout">
+        <item row="0" column="1">
+         <widget class="QDoubleSpinBox" name="j1_spinbox">
+          <property name="decimals">
+           <number>4</number>
+          </property>
+          <property name="minimum">
+           <double>-10000.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>10000.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="0">
+         <widget class="QLabel" name="label_2">
+          <property name="text">
+           <string>J1</string>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="1">
+         <widget class="QDoubleSpinBox" name="j2_spinbox">
+          <property name="decimals">
+           <number>4</number>
+          </property>
+          <property name="minimum">
+           <double>-10000.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>10000.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QDoubleSpinBox" name="j3_spinbox">
+          <property name="decimals">
+           <number>4</number>
+          </property>
+          <property name="minimum">
+           <double>-10000.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>10000.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="1">
+         <widget class="QDoubleSpinBox" name="j4_spinbox">
+          <property name="decimals">
+           <number>4</number>
+          </property>
+          <property name="minimum">
+           <double>-10000.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>10000.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="4" column="1">
+         <widget class="QDoubleSpinBox" name="j5_spinbox">
+          <property name="decimals">
+           <number>4</number>
+          </property>
+          <property name="minimum">
+           <double>-10000.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>10000.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="5" column="1">
+         <widget class="QDoubleSpinBox" name="j6_spinbox">
+          <property name="decimals">
+           <number>4</number>
+          </property>
+          <property name="minimum">
+           <double>-10000.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>10000.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="0">
+         <widget class="QLabel" name="label_3">
+          <property name="text">
+           <string>J2</string>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="0">
+         <widget class="QLabel" name="label_4">
+          <property name="text">
+           <string>J3</string>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="0">
+         <widget class="QLabel" name="label_5">
+          <property name="text">
+           <string>J4</string>
+          </property>
+         </widget>
+        </item>
+        <item row="4" column="0">
+         <widget class="QLabel" name="label_6">
+          <property name="text">
+           <string>J5</string>
+          </property>
+         </widget>
+        </item>
+        <item row="5" column="0">
+         <widget class="QLabel" name="label_7">
+          <property name="text">
+           <string>J6</string>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+      <item>
+       <widget class="QPushButton" name="manual_set_positions_pushbutton">
+        <property name="text">
+         <string>Set Positions</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <spacer name="verticalSpacer_2">
+        <property name="orientation">
+         <enum>Qt::Vertical</enum>
+        </property>
+        <property name="sizeHint" stdset="0">
+         <size>
+          <width>20</width>
+          <height>40</height>
+         </size>
+        </property>
+       </spacer>
+      </item>
+     </layout>
+    </item>
+    <item row="0" column="0">
+     <layout class="QVBoxLayout" name="verticalLayout_4">
+      <item>
+       <layout class="QHBoxLayout" name="horizontalLayout">
+        <item>
+         <widget class="QLabel" name="label_10">
+          <property name="font">
+           <font>
+            <pointsize>18</pointsize>
+            <weight>75</weight>
+            <bold>true</bold>
+           </font>
+          </property>
+          <property name="text">
+           <string>DENSO Pendant</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <spacer name="horizontalSpacer">
+          <property name="orientation">
+           <enum>Qt::Horizontal</enum>
+          </property>
+          <property name="sizeHint" stdset="0">
+           <size>
+            <width>40</width>
+            <height>20</height>
+           </size>
+          </property>
+         </spacer>
+        </item>
+        <item>
+         <widget class="QLabel" name="motor_power_label">
+          <property name="styleSheet">
+           <string notr="true">background-color: darkred;</string>
+          </property>
+          <property name="text">
+           <string>MOTOR POWER OFF</string>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+      <item>
+       <layout class="QVBoxLayout" name="verticalLayout_5">
+        <item>
+         <widget class="QLabel" name="label">
+          <property name="font">
+           <font>
+            <pointsize>12</pointsize>
+            <weight>75</weight>
+            <bold>true</bold>
+           </font>
+          </property>
+          <property name="text">
+           <string>Max Speed Limit</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout_3">
+          <item>
+           <widget class="QLabel" name="label_11">
+            <property name="font">
+             <font>
+              <pointsize>10</pointsize>
+              <weight>75</weight>
+              <bold>true</bold>
+             </font>
+            </property>
+            <property name="text">
+             <string>Current Max Speed:</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QLabel" name="max_speed_label">
+            <property name="text">
+             <string>0</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout_2">
+          <item>
+           <widget class="QSpinBox" name="speed_limit_spinbox">
+            <property name="minimum">
+             <number>1</number>
+            </property>
+            <property name="maximum">
+             <number>100</number>
+            </property>
+            <property name="value">
+             <number>5</number>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QPushButton" name="speed_limit_apply_pushbutton">
+            <property name="text">
+             <string>Apply</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+       </layout>
+      </item>
+      <item>
+       <layout class="QVBoxLayout" name="verticalLayout_3">
+        <item>
+         <widget class="QLabel" name="label_9">
+          <property name="font">
+           <font>
+            <pointsize>12</pointsize>
+            <weight>75</weight>
+            <bold>true</bold>
+           </font>
+          </property>
+          <property name="text">
+           <string>Control Mode</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QRadioButton" name="control_mode_stored_points_radiobutton">
+          <property name="text">
+           <string>Stored Points</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QRadioButton" name="control_mode_manual_radiobutton">
+          <property name="text">
+           <string>Manual Joint Control</string>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+      <item>
+       <layout class="QVBoxLayout" name="verticalLayout_2">
+        <item>
+         <widget class="QLabel" name="label_12">
+          <property name="font">
+           <font>
+            <pointsize>12</pointsize>
+            <weight>75</weight>
+            <bold>true</bold>
+           </font>
+          </property>
+          <property name="text">
+           <string>Stored Points</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QPushButton" name="movep0_pushbutton">
+          <property name="text">
+           <string>Move P0</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QPushButton" name="movep1_pushbutton">
+          <property name="text">
+           <string>Move P1</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QPushButton" name="movep2_pushbutton">
+          <property name="text">
+           <string>Move P2</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <spacer name="verticalSpacer">
+          <property name="orientation">
+           <enum>Qt::Vertical</enum>
+          </property>
+          <property name="sizeHint" stdset="0">
+           <size>
+            <width>20</width>
+            <height>40</height>
+           </size>
+          </property>
+         </spacer>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </item>
+   </layout>
+  </widget>
+  <widget class="QStatusBar" name="statusbar"/>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

OSU Coursework/ROB 421 - Applied Robotics/tools/pendant/pendant.py

@@ -0,0 +1,164 @@
+# coding:utf-8
+#####################################
+# Imports
+#####################################
+# Python native imports
+import sys
+from PyQt5 import QtWidgets, QtCore, QtGui, uic
+import signal
+import qdarkstyle
+
+from Framework.cao_controller import CAOController
+from Framework.cao_monitor import CAOMonitor
+
+#####################################
+# Global Variables
+#####################################
+UI_FILE = "Resources/Ui/pendant.ui"
+
+
+# # CAO Engine
+# eng = win32com.client.Dispatch("CAO.CaoEngine")
+#
+# ctrl = eng.Workspaces(0).AddController("RC", "CaoProv.DENSO.NetwoRC", "", "conn=eth:192.168.1.10")
+# Arm1 = ctrl.AddRobot("Arm1", "")
+#
+# # Get and print a variable
+# # p_val_0 = ctrl.AddVariable("P0", "")
+# # p_val_1 = ctrl.AddVariable("P1", "")
+# # p_val_2 = ctrl.AddVariable("P2", "")
+# # print("P0 is: ", p_val_0)
+# # print("P1 is: ", p_val_1)
+# # print("P2 is: ", p_val_2)
+#
+# ctrl.Execute("PutAutoMode", 2)  # Put controller into external auto mode
+#
+# Arm1.Execute("Motor", 1)  # Enable motor power
+#
+# try:
+#     Arm1.Execute("TakeArm", 0)
+# except Exception:
+#     Arm1.Execute("Motor", 0)
+#     exit()
+#
+# Arm1.Execute("ExtSpeed", (20, 20, 20))
+#
+# while True:
+#     try:
+#         Arm1.Move(1, "@P P0", "")
+#         Arm1.Move(1, "@P P1", "")
+#         Arm1.Move(1, "@P P2", "")
+#     except KeyboardInterrupt:
+#         Arm1.Execute("GiveArm", )
+#         Arm1.Execute("Motor", 0)
+#         exit()
+
+
+#####################################
+# 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):
+    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)
+
+        # ########## 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"]["main_screen"] = \
+            self.create_application_window(UI_FILE, "Denso Pendant",
+                                           self.SCREEN_ID)  # type: ApplicationWindow
+
+        # ##### Instantiate Regular Classes ######
+
+        # ##### Instantiate Threaded Classes ######
+        self.__add_thread("CAO Controller", CAOController(self.shared_objects))
+        self.__add_thread("CAO Monitor", CAOMonitor(self.shared_objects))
+
+        self.connect_signals_and_slots_signal.emit()
+        self.__connect_signals_to_slots()
+        self.start_threads_signal.emit()
+
+    def __add_thread(self, thread_name, instance):
+        self.shared_objects["threaded_classes"][thread_name] = instance
+        instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal,
+                               self.kill_threads_signal)
+
+    def __connect_signals_to_slots(self):
+        self.shared_objects["screens"]["main_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() | QtCore.Qt.WindowStaysOnTopHint)
+
+        # app_window.setGeometry(
+        #     system_desktop.screenGeometry(display_screen))  # Sets the window to be on the first screen
+
+        app_window.show()  # 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("pendant")
+
+    # ########## Start Ground Station If Ready ##########
+    ground_station = GroundStation()
+    application.exec_()  # Execute launching of the application