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

Added dummy movement for arm position. Changed rover_drive and iris_controller to handle full dropouts of the usb device (hopefully)...

Browse Source
This commit is contained in:
Corwin Perren
2018-03-13 17:59:59 -07:00
parent f9560e17d6
commit 3242f2d866
9 changed files with 214 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py Normal file
View File

@@ -0,0 +1,96 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets, QtGui
import logging
from time import time
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 10
ROTATION_SPEED_MODIFIER = 0.015
#####################################
# Controller Class Definition
#####################################
class ArmIndication(QtCore.QThread):
arm_joint_position_updated__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
super(ArmIndication, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.right_screen = self.shared_objects["screens"]["right_screen"]
self.base_rotation_dial = self.right_screen.base_rotation_dial # type: QtWidgets.QDial
self.shoulder_pitch_dial = self.right_screen.shoulder_pitch_dial # type: QtWidgets.QDial
self.elbow_pitch_dial = self.right_screen.elbow_pitch_dial # type: QtWidgets.QDial
self.elbow_roll_dial = self.right_screen.elbow_roll_dial # type: QtWidgets.QDial
self.end_effector_pitch_dial = self.right_screen.end_effector_pitch_dial # type: QtWidgets.QDial
self.end_effector_rotation_dial = self.right_screen.end_effector_rotation_dial # type: QtWidgets.QDial
# ########## 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.current_position_delta = 0
self.shown_position = 0
def run(self):
while self.run_thread_flag:
start_time = time()
self.change_position_if_needed()
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
def change_position_if_needed(self):
self.shown_position += self.current_position_delta * ROTATION_SPEED_MODIFIER
self.shown_position %= 360
self.arm_joint_position_updated__signal.emit(self.shown_position)
def __on_position_change_requested__slot(self, event):
if event.button() == QtCore.Qt.LeftButton:
self.current_position_delta = 1
elif event.button() == QtCore.Qt.RightButton:
self.current_position_delta = 0
def connect_signals_and_slots(self):
self.arm_joint_position_updated__signal.connect(self.base_rotation_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.shoulder_pitch_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.elbow_pitch_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.elbow_roll_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.end_effector_pitch_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.end_effector_rotation_dial.setValue)
self.base_rotation_dial.mousePressEvent = self.__on_position_change_requested__slot
self.shoulder_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
self.elbow_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
self.elbow_roll_dial.mousePressEvent = self.__on_position_change_requested__slot
self.end_effector_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
self.end_effector_rotation_dial.mousePressEvent = self.__on_position_change_requested__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

0
software/ros_packages/ground_station/src/Framework/ArmSystems/__init__.py Normal file
View File

9
software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py
View File

@@ -60,9 +60,6 @@ class SpeedAndHeadingIndication(QtCore.QThread):
self.current_heading_shown_rotation_angle = 0
self.rotation_direction = 1
# compass_image = PIL.Image.open("Resources/Images/compass.png").resize((300, 300)).rotate(45) # PIL.Image
# self.shared_objects["screens"]["right_screen"].heading_compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image)))
def run(self):
self.on_heading_changed__slot(self.current_heading)
@@ -107,16 +104,14 @@ class SpeedAndHeadingIndication(QtCore.QThread):
def __on_heading_clicked__slot(self, event):
new_heading = self.current_heading
if event.button() == QtCore.Qt.LeftButton:
new_heading = (self.current_heading + 10) % 360
new_heading = (self.current_heading + 5) % 360
elif event.button() == QtCore.Qt.RightButton:
new_heading = (self.current_heading - 10) % 360
new_heading = (self.current_heading - 5) % 360
self.on_heading_changed__slot(new_heading)
self.new_speed_update_ready__signal.emit("%.2f" % (random() * 2.5))
self.heading_text_update_ready__signal.emit(str(new_heading) + "°")
def on_new_compass_image_ready__slot(self):
self.heading_compass_label.setPixmap(self.compass_pixmap)

123
software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui
View File

@@ -150,7 +150,7 @@
</widget>
</item>
<item>
<widget class="QDial" name="dial_31">
<widget class="QDial" name="base_rotation_dial">
<property name="minimumSize">
<size>
<width>305</width>
@@ -158,10 +158,10 @@
</size>
</property>
<property name="minimum">
<number>-180</number>
<number>0</number>
</property>
<property name="maximum">
<number>180</number>
<number>360</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -217,7 +217,7 @@
</widget>
</item>
<item>
<widget class="QDial" name="dial_32">
<widget class="QDial" name="shoulder_pitch_dial">
<property name="minimumSize">
<size>
<width>305</width>
@@ -225,10 +225,10 @@
</size>
</property>
<property name="minimum">
<number>-180</number>
<number>0</number>
</property>
<property name="maximum">
<number>180</number>
<number>360</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -258,7 +258,7 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>true</bool>
<bool>false</bool>
</property>
</widget>
</item>
@@ -268,19 +268,6 @@
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_11">
<item>
<spacer name="horizontalSpacer_7">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_41">
<item>
@@ -301,7 +288,7 @@
</widget>
</item>
<item>
<widget class="QDial" name="dial_33">
<widget class="QDial" name="elbow_pitch_dial">
<property name="minimumSize">
<size>
<width>305</width>
@@ -309,10 +296,10 @@
</size>
</property>
<property name="minimum">
<number>-180</number>
<number>0</number>
</property>
<property name="maximum">
<number>180</number>
<number>360</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -342,24 +329,82 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>true</bool>
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer_8">
<layout class="QHBoxLayout" name="horizontalLayout_13">
<item>
<layout class="QVBoxLayout" name="verticalLayout_45">
<item>
<widget class="QLabel" name="label_50">
<property name="font">
<font>
<pointsize>14</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Elbow Roll</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QDial" name="elbow_roll_dial">
<property name="minimumSize">
<size>
<width>305</width>
<height>185</height>
</size>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>360</number>
</property>
<property name="pageStep">
<number>10</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="sliderPosition">
<number>0</number>
</property>
<property name="tracking">
<bool>true</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
<property name="invertedAppearance">
<bool>false</bool>
</property>
</spacer>
<property name="invertedControls">
<bool>false</bool>
</property>
<property name="wrapping">
<bool>true</bool>
</property>
<property name="notchTarget">
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</item>
@@ -385,7 +430,7 @@
</widget>
</item>
<item>
<widget class="QDial" name="dial_34">
<widget class="QDial" name="end_effector_pitch_dial">
<property name="minimumSize">
<size>
<width>305</width>
@@ -393,10 +438,10 @@
</size>
</property>
<property name="minimum">
<number>-180</number>
<number>0</number>
</property>
<property name="maximum">
<number>180</number>
<number>360</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -426,7 +471,7 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>true</bool>
<bool>false</bool>
</property>
</widget>
</item>
@@ -452,7 +497,7 @@
</widget>
</item>
<item>
<widget class="QDial" name="dial_35">
<widget class="QDial" name="end_effector_rotation_dial">
<property name="minimumSize">
<size>
<width>305</width>
@@ -460,10 +505,10 @@
</size>
</property>
<property name="minimum">
<number>-180</number>
<number>0</number>
</property>
<property name="maximum">
<number>180</number>
<number>360</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -493,7 +538,7 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>true</bool>
<bool>false</bool>
</property>
</widget>
</item>

3
software/ros_packages/ground_station/src/ground_station.py
View File

@@ -17,6 +17,7 @@ import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator
import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator
import Framework.JoystickControlSystems.JoystickControlSender as JoystickControlSender
import Framework.NavigationSystems.SpeedAndHeadingIndication as SpeedAndHeading
import Framework.ArmSystems.ArmIndication as ArmIndication
#####################################
# Global Variables
@@ -101,6 +102,8 @@ class GroundStation(QtCore.QObject):
self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects))
self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects))
self.__add_thread("Speed and Heading", SpeedAndHeading.SpeedAndHeadingIndication(self.shared_objects))
self.__add_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects))
self.connect_signals_and_slots_signal.emit()
self.__connect_signals_to_slots()
self.start_threads_signal.emit()

22
software/ros_packages/rover_control/src/drive_control/drive_control.py
View File

@@ -55,6 +55,8 @@ MOTOR_DRIVER_DEFAULT_MESSAGE = [
UINT16_MAX = 65535
BOGIE_LAST_SEEN_TIMEOUT = 2 # seconds
#####################################
# DriveControl Class Definition
@@ -75,9 +77,10 @@ class DriveControl(object):
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID)
self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID)
self.__setup_minimalmodbus_for_485()
self.first_motor = None
self.second_motor = None
self.connect_to_bogie()
self.drive_control_subscriber = \
rospy.Subscriber(self.drive_control_subscriber_topic, DriveControlMessage, self.drive_control_callback)
@@ -87,6 +90,8 @@ class DriveControl(object):
self.drive_control_message = DriveControlMessage()
self.new_control_message = False
self.bogie_last_seen = time()
self.run()
def __setup_minimalmodbus_for_485(self):
@@ -111,10 +116,18 @@ class DriveControl(object):
except Exception, error:
print "Error occurred:", error
if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT:
return # Exit so respawn can take over
time_diff = time() - start_time
sleep(max(self.wait_time - time_diff, 0))
def connect_to_bogie(self):
self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID)
self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID)
self.__setup_minimalmodbus_for_485()
def send_drive_control_message(self):
if self.new_control_message:
drive_control = self.drive_control_message
@@ -156,6 +169,9 @@ class DriveControl(object):
except Exception:
status.second_motor_connected = False
if status.first_motor_connected or status.second_motor_connected:
self.bogie_last_seen = time()
if status.first_motor_connected:
status.first_motor_current = first_motor_status[0] / 1000.0
status.first_motor_fault = first_motor_status[1]

1
software/ros_packages/rover_control/src/iris_controller/iris_controller.py
View File

@@ -111,6 +111,7 @@ class IrisController(object):
except Exception, error:
print "Error occurred:", error
return
time_diff = time() - start_time

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

@@ -1,17 +1,17 @@
<launch>
<group ns="rover_control">
<node name="iris_controller" pkg="rover_control" type="iris_controller.py" output="screen">
<node name="iris_controller" pkg="rover_control" type="iris_controller.py" respawn="true" output="screen">
<param name="port" value="/dev/rover/ttyIRIS"/>
<param name="hertz" value="20"/>
</node>
<node name="rear_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<node name="rear_bogie" pkg="rover_control" type="drive_control.py" respawn="true" output="screen">
<param name="port" value="/dev/rover/ttyBogieRear"/>
<param name="drive_control_topic" value="drive_control/rear"/>
<param name="drive_control_status_topic" value="drive_status/rear"/>
</node>
<node name="left_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<node name="left_bogie" pkg="rover_control" type="drive_control.py" respawn="true" output="screen">
<param name="port" value="/dev/rover/ttyBogieLeft"/>
<param name="drive_control_topic" value="drive_control/left"/>
<param name="drive_control_status_topic" value="drive_status/left"/>
@@ -19,7 +19,7 @@
<param name="invert_second_motor" value="True"/>
</node>
<node name="right_bogie" pkg="rover_control" type="drive_control.py" output="screen">
<node name="right_bogie" pkg="rover_control" type="drive_control.py" respawn="true" output="screen">
<param name="port" value="/dev/rover/ttyBogieRight"/>
<param name="drive_control_topic" value="drive_control/right"/>
<param name="drive_control_status_topic" value="drive_status/right"/>

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

@@ -106,7 +106,7 @@
{name: "/rover_status/camera_status", compress: true, rate: 1.0},
{name: "/rover_status/frsky_status", compress: true, rate: 1.0},
{name: "/rover_status/gps_status", compress: true, rate: 1.0},
{name: "/rover_status/jetson_status", compress: true, rate: 5.0},
{name: "/rover_status/jetson_status", compress: true, rate: 0.2},
{name: "/rover_status/misc_status", compress: true, rate: 1.0}]
</rosparam>
</node>