From 368401176b22f21268aa00b85bc7afa9b1003331 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Thu, 12 Apr 2018 11:15:24 -0700 Subject: [PATCH] Added error handling for missing radio. More spacenav changes, testing nav launch file. --- .../InputSystems/SpaceNavControlSender.py | 19 ++- .../SettingsSystems/UbiquitiRadioSettings.py | 7 +- software/testing/nav_testing/nav.launch | 2 +- software/testing/nav_testing/rviz config.rviz | 147 ++++++++++++++++++ 4 files changed, 170 insertions(+), 5 deletions(-) create mode 100644 software/testing/nav_testing/rviz config.rviz diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py index 8afd46f..891b331 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py @@ -19,6 +19,9 @@ THREAD_HERTZ = 15 # Controller Class Definition ##################################### class SpaceNavControlSender(QtCore.QThread): + GUI_MODE = 0 + ARM_MODE = 1 + def __init__(self, shared_objects): super(SpaceNavControlSender, self).__init__() @@ -90,6 +93,10 @@ class SpaceNavControlSender(QtCore.QThread): 5: "f_pressed" } + + + self.current_control_mode = self.GUI_MODE + def run(self): spnav.spnav_open() @@ -108,20 +115,26 @@ class SpaceNavControlSender(QtCore.QThread): if event: if event.ev_type == spnav.SPNAV_EVENT_MOTION: - # FIXME: Make sure these divisors are correct. Should be for most. Check linear_z, is weird 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 + 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.GUI_MODE + elif self.spnav_states["2_pressed"]: + self.current_control_mode = self.ARM_MODE + + def connect_signals_and_slots(self): pass diff --git a/software/ros_packages/ground_station/src/Framework/SettingsSystems/UbiquitiRadioSettings.py b/software/ros_packages/ground_station/src/Framework/SettingsSystems/UbiquitiRadioSettings.py index 2d62166..96de1c5 100644 --- a/software/ros_packages/ground_station/src/Framework/SettingsSystems/UbiquitiRadioSettings.py +++ b/software/ros_packages/ground_station/src/Framework/SettingsSystems/UbiquitiRadioSettings.py @@ -58,7 +58,12 @@ class UbiquitiRadioSettings(QtCore.QThread): def run(self): self.set_gui_elements_enabled__signal.emit(False) - self.setup_and_connect_ssh_client() + + try: + self.setup_and_connect_ssh_client() + except Exception: + return + self.get_and_show_current_channel() while self.run_thread_flag: diff --git a/software/testing/nav_testing/nav.launch b/software/testing/nav_testing/nav.launch index eab0804..c0ffc28 100644 --- a/software/testing/nav_testing/nav.launch +++ b/software/testing/nav_testing/nav.launch @@ -2,7 +2,7 @@ - + diff --git a/software/testing/nav_testing/rviz config.rviz b/software/testing/nav_testing/rviz config.rviz new file mode 100644 index 0000000..b620534 --- /dev/null +++ b/software/testing/nav_testing/rviz config.rviz @@ -0,0 +1,147 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 1113 + - 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: "" +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: + Value: true + - Angle Tolerance: 0.100000001 + 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: 100 + Name: Odometry + Position Tolerance: 0.100000001 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Color: 255; 25; 0 + Head Length: 0.300000012 + Head Radius: 0.100000001 + Shaft Length: 1 + Shaft Radius: 0.0500000007 + Value: Arrow + Topic: /odometry/filtered + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + 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: 14.6344223 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.49345744 + Y: -0.492769718 + Z: -0.707593679 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 1.35979652 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.07858229 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1401 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004e1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000004e1000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000004e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000004e10000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003510000003efc0100000002fb0000000800540069006d00650100000000000003510000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000000ef000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 849 + X: 2787 + Y: 18