From 047c04ca41f734c68b3b47595b6bc38fb1d98849 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Thu, 2 Aug 2018 00:55:44 -0700 Subject: [PATCH] Started adding in xbox controller to gui. Thinking of scrapping spacenav mouse. It kinda sucks. Also, three axes moving on the arm! Time to make an action client for moveitgit add .git add .! --- .../UDEV_rules/99-rover-usb-serial.rules | 4 + .../InputSystems/ControllerControlSender.py | 168 ++++++++++++++++++ .../InputSystems/JoystickControlSender.py | 4 + .../InputSystems/SpaceNavControlSender.py | 4 + .../src/Framework/MiscSystems/MiningCore.py | 6 +- .../ground_station/src/ground_station.py | 2 + .../ros_packages/rover_arm/src/rover_arm.cpp | 34 +++- .../rover_arm/src/simplemotion/busdevice.c | 2 +- .../rover/topic_transport_senders.launch | 2 +- 9 files changed, 218 insertions(+), 8 deletions(-) create mode 100644 software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py diff --git a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules index 418b174..901ff80 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules @@ -7,6 +7,8 @@ ######################### +##### NOTE!!!! The IONIs cannot be on IRIS. The throughput is too high for it to handle with only a 12Mbit usb hub onboard ##### + # IRIS Board Mappings SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyREAR" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyLEFT" @@ -23,6 +25,8 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyExtraUART" +# External 485 adapter for the ARM +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyARM" ##### Mappings from OMSI / EXPO driving with individual adapters # SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyLEFT" diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py new file mode 100644 index 0000000..c3c7705 --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py @@ -0,0 +1,168 @@ +##################################### +# Imports +##################################### +# Python native imports +from PyQt5 import QtCore, QtWidgets +import logging +from inputs import devices, GamePad +from time import time + +import rospy +from rover_control.msg import DriveCommandMessage, TowerPanTiltControlMessage + +##################################### +# Global Variables +##################################### +GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360" + +DRIVE_COMMAND_HERTZ = 20 + + +##################################### +# Controller Class Definition +##################################### +class XBOXController(QtCore.QThread): + def __init__(self): + super(XBOXController, self).__init__() + + # ########## Thread Flags ########## + self.run_thread_flag = True + self.setup_controller_flag = True + self.data_acquisition_and_broadcast_flag = True + self.controller_acquired = False + + # ########## Class Variables ########## + self.gamepad = None # type: GamePad + + self.controller_states = { + "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: + print device + if device.name == GAME_CONTROLLER_NAME: + self.gamepad = device + + return True + return False + + def __get_controller_data(self): + if self.controller_acquired: + events = self.gamepad.read() + + for event in events: + print event.code + if event.code in self.raw_mapping_to_class_mapping: + self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state + + self.ready = True + + +##################################### +# Controller Class Definition +##################################### +class ControllerControlSender(QtCore.QThread): + + def __init__(self, shared_objects): + super(ControllerControlSender, self).__init__() + + # ########## Reference to class init variables ########## + self.shared_objects = shared_objects + self.video_coordinator = self.shared_objects["threaded_classes"]["Video Coordinator"] + self.right_screen = self.shared_objects["screens"]["right_screen"] + + # ########## 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 = False + + self.controller = XBOXController() + + # ########## Class Variables ########## + + self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ + + def run(self): + self.logger.debug("Starting Joystick Thread") + + while self.run_thread_flag: + start_time = time() + + time_diff = time() - start_time + + self.msleep(max(int(self.wait_time - time_diff), 0)) + + self.logger.debug("Stopping Joystick Thread") + + def connect_signals_and_slots(self): + 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 diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py index 7070752..99b3bee 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py @@ -193,6 +193,8 @@ class JoystickControlSender(QtCore.QThread): self.last_camera_toggle_time = time() def run(self): + self.logger.debug("Starting Joystick Thread") + while self.run_thread_flag: start_time = time() @@ -203,6 +205,8 @@ class JoystickControlSender(QtCore.QThread): self.msleep(max(int(self.wait_time - time_diff), 0)) + self.logger.debug("Stopping Joystick Thread") + def connect_signals_and_slots(self): self.set_speed_limit__signal.connect(self.speed_limit_progress_bar.setValue) self.set_left_drive_output__signal.connect(self.left_drive_progress_bar.setValue) 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 43fd862..9eca7ca 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py @@ -111,6 +111,7 @@ class SpaceNavControlSender(QtCore.QThread): self.current_control_mode = self.MINING_MODE def run(self): + self.logger.debug("Starting SpaceNav Mouse Thread") spnav.spnav_open() while self.run_thread_flag: @@ -125,10 +126,13 @@ class SpaceNavControlSender(QtCore.QThread): # self.msleep(max(int((self.wait_time - time_diff) * 1000), 0)) + self.logger.debug("Stopping SpaceNav Mouse Thread") + def process_spnav_events(self): event = spnav.spnav_poll_event() if event: + # print event if event.ev_type == spnav.SPNAV_EVENT_MOTION: self.spnav_states["linear_x"] = event.translation[0] / 350.0 self.spnav_states["linear_y"] = event.translation[2] / 350.0 diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py index 87f8aec..2967871 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py @@ -33,6 +33,8 @@ class Mining(QtCore.QObject): lift_position_update_ready__signal = QtCore.pyqtSignal(int) tilt_position_update_ready__signal = QtCore.pyqtSignal(int) + weight_measurement_update_ready__signal = QtCore.pyqtSignal(int) + def __init__(self, shared_objects): super(Mining, self).__init__() @@ -81,6 +83,8 @@ class Mining(QtCore.QObject): self.tilt_position_update_ready__signal.connect(self.tilt_position_progress_bar.setValue) self.lift_position_update_ready__signal.connect(self.lift_position_progress_bar.setValue) + self.weight_measurement_update_ready__signal.connect(self.mining_qlcdnumber.display) + def on_mining_set_cal_factor_clicked__slot(self): message = MiningControlMessage() @@ -136,4 +140,4 @@ class Mining(QtCore.QObject): status = status # type:MiningStatusMessage self.tilt_position_update_ready__signal.emit(status.tilt_position) self.lift_position_update_ready__signal.emit(status.lift_position) - self.mining_qlcdnumber.display(status.measured_weight) + self.weight_measurement_update_ready__signal.emit(status.measured_weight) diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index 7be7e2d..b1facf9 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/software/ros_packages/ground_station/src/ground_station.py @@ -16,6 +16,7 @@ import Framework.LoggingSystems.Logger as Logger import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator import Framework.InputSystems.JoystickControlSender as JoystickControlSender +import Framework.InputSystems.ControllerControlSender as ControllerControlSender import Framework.NavigationSystems.SpeedAndHeadingIndication as SpeedAndHeading import Framework.NavigationSystems.WaypointsCoordinator as WaypointsCoordinator import Framework.ArmSystems.ArmIndication as ArmIndication @@ -108,6 +109,7 @@ class GroundStation(QtCore.QObject): self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects)) self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects)) self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects)) + self.__add_thread("Controller Sender", ControllerControlSender.ControllerControlSender(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.__add_thread("Rover Status", StatusCore.SensorCore(self.shared_objects)) diff --git a/software/ros_packages/rover_arm/src/rover_arm.cpp b/software/ros_packages/rover_arm/src/rover_arm.cpp index cf134c2..2848d46 100644 --- a/software/ros_packages/rover_arm/src/rover_arm.cpp +++ b/software/ros_packages/rover_arm/src/rover_arm.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -13,8 +14,7 @@ int device_address = 1; ////////// Global Variables ////////// // ROS Parameter Defaults -const string default_port = "/dev/ttyUSB0"; - +const string default_port = "/dev/rover/ttyARM"; // Axis Defaults const smint32 axis1_max_count = 100000; @@ -47,6 +47,8 @@ public: if(arm_bus_handle < 0){ ROS_ERROR("Could not connect to arm"); } + + ROS_INFO("Connected to smBus"); } void run(){ @@ -57,18 +59,40 @@ public: while(ros::ok()){ smSetParameter(arm_bus_handle, device_address, SMP_FAULTS, 0); smSetParameter(arm_bus_handle, device_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + smSetParameter(arm_bus_handle, 2, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, 2, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + smSetParameter(arm_bus_handle, 3, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, 3, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); smint32 status = 0; - while(!(status & STAT_TARGET_REACHED)){ - smRead1Parameter(arm_bus_handle, device_address, SMP_STATUS, &status); + smint32 status1 = 0; + smint32 status2 = 0; + + smint32 position = 0; + smint32 position1 = 0; + smint32 position2 = 0; + + smint32 read_bus_voltage; + + while(!(status & STAT_TARGET_REACHED) || !(status1 & STAT_TARGET_REACHED) || !(status2 & STAT_TARGET_REACHED)){ + smRead2Parameters(arm_bus_handle, device_address, SMP_STATUS, &status, SMP_ACTUAL_POSITION_FB, &position); + smRead2Parameters(arm_bus_handle, 2, SMP_STATUS, &status1, SMP_ACTUAL_POSITION_FB, &position1); + smRead2Parameters(arm_bus_handle, 3, SMP_STATUS, &status2, SMP_ACTUAL_POSITION_FB, &position2); + smRead1Parameter(arm_bus_handle, device_address, SMP_ACTUAL_BUS_VOLTAGE, &read_bus_voltage); + + ROS_INFO("Dir: %u\tVoltage: %ld\tAxis 1: %ld\tAxis 2: %ld\tAxis 3: %ld", dir, read_bus_voltage, position, position1, position2); } dir = !dir; if(dir){ smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 0); + smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 0); + smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 0); }else{ - smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 330000); + smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 50000); + smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 300000); + smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 250000); } ros::spinOnce(); diff --git a/software/ros_packages/rover_arm/src/simplemotion/busdevice.c b/software/ros_packages/rover_arm/src/simplemotion/busdevice.c index 9bf82c8..5b43ddc 100644 --- a/software/ros_packages/rover_arm/src/simplemotion/busdevice.c +++ b/software/ros_packages/rover_arm/src/simplemotion/busdevice.c @@ -63,7 +63,7 @@ smbusdevicehandle smBDOpen( const char *devicename ) //all handles in use if(handle>=SM_MAX_BUSES) return -1; - if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0) //use rs232 lib + if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0 || strncmp(devicename,"/dev/rover/tty",8) == 0) { BusDevice[handle].comPort=OpenComport( devicename, SM_BAUDRATE ); if( BusDevice[handle].comPort == -1 ) diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index 88e9805..3ce6974 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch @@ -155,7 +155,7 @@ {name: "/rover_status/battery_status", compress: false, rate: 1.0}, {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0}, {name: "/rover_odometry/imu/data", compress: false, rate: 10.0}, - {name: "/rover_control/mining/status", compress: false, rate: 2.0}, + {name: "/rover_control/mining/status", compress: false, rate: 5.0}, ]