diff --git a/software/firmware/tower/tower.ino b/software/firmware/tower/tower.ino index 1f4410d..f029aba 100644 --- a/software/firmware/tower/tower.ino +++ b/software/firmware/tower/tower.ino @@ -131,6 +131,7 @@ void loop() { poll_modbus(); set_leds(); send_imu_stream_line(root); + add_cal_status(root); process_gps_and_send_if_ready(root); process_white_led_command(); get_co2_data(); @@ -221,31 +222,17 @@ void setup_imu(){ // } } -void display_cal_status(void) +void add_cal_status(JsonObject &root) { - /* Get the four calibration values (0..3) */ - /* Any sensor data reporting 0 should be ignored, */ - /* 3 means 'fully calibrated" */ - uint8_t system, gyro, accel, mag; - system = gyro = accel = mag = 0; - bno.getCalibration(&system, &gyro, &accel, &mag); + JsonObject& imu_cal_object = root.createNestedObject("imu_cal"); + uint8_t system, gyro, accel, mag; + system = gyro = accel = mag = 0; - /* The data should be ignored until the system calibration is > 0 */ - Serial.print("\t"); - if (!system) - { - Serial.print("! "); - } + bno.getCalibration(&system, &gyro, &accel, &mag); - /* Display the individual values */ - Serial.print("Sys:"); - Serial.print(system, DEC); - Serial.print(" G:"); - Serial.print(gyro, DEC); - Serial.print(" A:"); - Serial.print(accel, DEC); - Serial.print(" M:"); - Serial.println(mag, DEC); + imu_cal_object["gyro"] = gyro; + imu_cal_object["accel"] = accel; + imu_cal_object["mag"] = mag; } void process_gps_and_send_if_ready(JsonObject &root) { diff --git a/software/ground_station_setup.sh b/software/ground_station_setup.sh index efe1a9d..87c26f5 100755 --- a/software/ground_station_setup.sh +++ b/software/ground_station_setup.sh @@ -14,6 +14,7 @@ folders_to_link=( rover_camera rover_status rover_arm + rover_science ) # Print heading diff --git a/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py b/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py index b033f33..7b60e41 100644 --- a/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py +++ b/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py @@ -106,6 +106,8 @@ class ArmIndication(QtCore.QObject): thumb_current_updated__signal = QtCore.pyqtSignal(int) middlefinger_current_updated__signal = QtCore.pyqtSignal(int) + gripper_reported_setpoint_updated__signal = QtCore.pyqtSignal(int) + def __init__(self, shared_objects): super(ArmIndication, self).__init__() @@ -153,6 +155,8 @@ class ArmIndication(QtCore.QObject): self.arm_controls_wrist_roll_status_label = self.right_screen.arm_controls_wrist_roll_status_label # type:QtWidgets.QLabel self.arm_controls_wrist_roll_faults_label = self.right_screen.arm_controls_wrist_roll_faults_label # type:QtWidgets.QLabel + self.gripper_reported_setpoint_lcd_number = self.right_screen.gripper_reported_setpoint_lcd_number # type: QtWidgets.QLCDNumber + # ########## Get the settings instance ########## self.settings = QtCore.QSettings() @@ -205,6 +209,8 @@ class ArmIndication(QtCore.QObject): self.thumb_current_updated__signal.connect(self.thumb_current_lcd_number.display) self.middlefinger_current_updated__signal.connect(self.middlefinger_current_lcd_number.display) + self.gripper_reported_setpoint_updated__signal.connect(self.gripper_reported_setpoint_lcd_number.display) + def on_arm_status_update_received__callback(self, data): self.base_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.base_comm_status)) self.shoulder_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.shoulder_comm_status)) @@ -248,6 +254,8 @@ class ArmIndication(QtCore.QObject): self.thumb_current_updated__signal.emit(data.thumb_current) self.middlefinger_current_updated__signal.emit(data.middlefinger_current) + self.gripper_reported_setpoint_updated__signal.emit(data.current_finger_position) + @staticmethod def process_faults_to_string(faults): fault_output = "" diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py index 17cda1d..1a1726a 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py @@ -138,7 +138,7 @@ class LogitechJoystick(QtCore.QThread): events = self.gamepad.read() for event in events: - print event.code, event.state + # print event.code, event.state if event.code in self.raw_mapping_to_class_mapping: self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state @@ -164,7 +164,7 @@ class LogitechControllerControlSender(QtCore.QThread): self.shared_objects = shared_objects self.video_coordinator = self.shared_objects["threaded_classes"]["Video Coordinator"] self.right_screen = self.shared_objects["screens"]["right_screen"] - self.speed_limit_progress_bar = self.right_screen.speed_limit_progress_bar # type: QtWidgets.QProgressBar + self.rover_speed_limit_slider = self.right_screen.rover_speed_limit_slider # type: QtWidgets.QSlider self.left_drive_progress_bar = self.right_screen.left_drive_progress_bar # type: QtWidgets.QProgressBar self.right_drive_progress_bar = self.right_screen.right_drive_progress_bar # type: QtWidgets.QProgressBar @@ -219,11 +219,11 @@ class LogitechControllerControlSender(QtCore.QThread): 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) self.set_right_drive_output__signal.connect(self.right_drive_progress_bar.setValue) self.video_coordinator.pan_tilt_selection_changed__signal.connect(self.on_pan_tilt_selection_changed__slot) + self.rover_speed_limit_slider.valueChanged.connect(self.on_speed_limit_slider_value_changed__slot) def check_and_set_pause_state(self): thumb_pressed = self.joystick.controller_states["start"] @@ -238,18 +238,15 @@ class LogitechControllerControlSender(QtCore.QThread): self.publish_pan_tilt_control_commands() def publish_drive_command(self): - throttle_axis = 1 - # throttle_axis = max((255 - self.joystick.controller_states["throttle_axis"]) / 255.0, THROTTLE_MIN) - if self.drive_paused: drive_message = DriveCommandMessage() else: - drive_message = self.get_drive_message(throttle_axis) + drive_message = self.get_drive_message(self.speed_limit) left_output = abs(drive_message.drive_twist.linear.x - drive_message.drive_twist.angular.z) right_output = abs(drive_message.drive_twist.linear.x + drive_message.drive_twist.angular.z) - self.set_speed_limit__signal.emit(throttle_axis * 100) + self.set_speed_limit__signal.emit(self.speed_limit * 100) self.set_left_drive_output__signal.emit(left_output * 100) self.set_right_drive_output__signal.emit(right_output * 100) @@ -303,14 +300,14 @@ class LogitechControllerControlSender(QtCore.QThread): pan_tilt_message.relative_tilt_adjustment = -(hat_y * CHASSIS_PAN_TILT_Y_AXIS_SCALAR) self.chassis_pan_tilt_command_publisher.publish(pan_tilt_message) - def get_drive_message(self, throttle_axis): + def get_drive_message(self, speed_limit): drive_message = DriveCommandMessage() left_y_axis = self.joystick.controller_states["left_y_axis"] if abs(self.joystick.controller_states["left_y_axis"]) > STICK_DEADBAND else 0 right_y_axis = self.joystick.controller_states["right_y_axis"] if abs(self.joystick.controller_states["right_y_axis"]) > STICK_DEADBAND else 0 - left_y_axis = throttle_axis * (-(left_y_axis - STICK_OFFSET) / STICK_MAX) - right_y_axis = throttle_axis * (-(right_y_axis - STICK_OFFSET) / STICK_MAX) + left_y_axis = speed_limit * (-(left_y_axis - STICK_OFFSET) / STICK_MAX) + right_y_axis = speed_limit * (-(right_y_axis - STICK_OFFSET) / STICK_MAX) drive_message.drive_twist.linear.x = (left_y_axis + right_y_axis) / 2.0 drive_message.drive_twist.angular.z = (right_y_axis - left_y_axis) / 2.0 @@ -320,6 +317,9 @@ class LogitechControllerControlSender(QtCore.QThread): def on_pan_tilt_selection_changed__slot(self, selection): self.current_pan_tilt_selection = selection + def on_speed_limit_slider_value_changed__slot(self, value): + self.speed_limit = value / 100.0 + def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): start_signal.connect(self.start) signals_and_slots_signal.connect(self.connect_signals_and_slots) diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py index 12eb07d..d88f0b1 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py @@ -29,7 +29,7 @@ ROLL_SCALAR = 0.003 WRIST_PITCH_SCALAR = 0.003 WRIST_ROLL_SCALAR = 0.006 -GRIPPER_MOVEMENT_SCALAR = 300 +GRIPPER_MOVEMENT_SCALAR = 1500 LEFT_X_AXIS_DEADZONE = 1500 LEFT_Y_AXIS_DEADZONE = 1500 @@ -180,6 +180,8 @@ class XBOXControllerControlSender(QtCore.QThread): "SCISSOR": 4 } + PINCH_MODE_ABSOLUTE_SET_POSITION = 57740 + def __init__(self, shared_objects): super(XBOXControllerControlSender, self).__init__() @@ -222,6 +224,7 @@ class XBOXControllerControlSender(QtCore.QThread): self.last_xbox_button_state = 0 self.last_left_bumper_state = 0 self.last_right_bumper_state = 0 + self.last_back_button_state = 0 self.gripper_control_mode = 0 self.gripper_control_mode_just_changed = False @@ -253,7 +256,8 @@ class XBOXControllerControlSender(QtCore.QThread): "SET": [self.gripper_mode_scissor_stylesheet_update_ready__signal], "UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal, self.gripper_mode_wide_stylesheet_update_ready__signal, - self.gripper_mode_normal_stylesheet_update_ready__signal] + self.gripper_mode_normal_stylesheet_update_ready__signal], + "ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION } } @@ -266,6 +270,7 @@ class XBOXControllerControlSender(QtCore.QThread): self.change_control_state_if_needed() if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"): + self.send_gripper_home_on_back_press() self.process_and_send_arm_control() elif self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("MINING"): self.send_mining_commands() @@ -331,7 +336,13 @@ class XBOXControllerControlSender(QtCore.QThread): for signal in signal_map["UNSET"]: signal.emit(COLOR_NONE) - self.send_new_gripper_mode = True + if "ABS_MOVE" in signal_map: + gripper_control_message = GripperControlMessage() + gripper_control_message.gripper_mode = self.GRIPPER_CONTROL_MODES["SCISSOR"] + gripper_control_message.gripper_position_absolute = signal_map["ABS_MOVE"] + self.gripper_control_publisher.publish(gripper_control_message) + else: + self.send_new_gripper_mode = True self.gripper_control_mode_just_changed = False @@ -340,12 +351,17 @@ class XBOXControllerControlSender(QtCore.QThread): arm_control_message = ArmControlMessage() gripper_control_message = GripperControlMessage() - gripper_control_message.gripper_position_absolute = -1 - gripper_control_message.gripper_mode = self.gripper_control_mode + 1 should_publish_arm = False should_publish_gripper = True if self.send_new_gripper_mode else False + if self.send_new_gripper_mode: + gripper_control_message.gripper_position_absolute = 0 + else: + gripper_control_message.gripper_position_absolute = -1 + + gripper_control_message.gripper_mode = self.gripper_control_mode + 1 + left_trigger = self.controller.controller_states["left_trigger"] right_trigger = self.controller.controller_states["right_trigger"] @@ -386,6 +402,18 @@ class XBOXControllerControlSender(QtCore.QThread): self.gripper_control_publisher.publish(gripper_control_message) self.send_new_gripper_mode = False + def send_gripper_home_on_back_press(self): + gripper_control_message = GripperControlMessage() + back_state = self.controller.controller_states["back_button"] + + if self.last_back_button_state == 0 and back_state == 1: + gripper_control_message.should_home = True + gripper_control_message.gripper_mode = 1 + self.gripper_control_publisher.publish(gripper_control_message) + self.last_back_button_state = 1 + elif self.last_back_button_state == 1 and back_state == 0: + self.last_back_button_state = 0 + def send_mining_commands(self): left_y_axis = self.controller.controller_states["left_y_axis"] if abs( 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 2967871..77a21ed 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py @@ -8,6 +8,7 @@ import logging import rospy from rover_control.msg import MiningStatusMessage, MiningControlMessage +from rover_science.msg import SoilSensorStatusMessage ##################################### # Global Variables @@ -15,6 +16,8 @@ from rover_control.msg import MiningStatusMessage, MiningControlMessage MINING_STATUS_TOPIC = "/rover_control/mining/status" MINING_CONTROL_TOPIC = "/rover_control/mining/control" +SOIL_PROBE_TOPIC = "/rover_science/soil_probe/data" + TRAVEL_POSITION_LIFT = 110 TRAVEL_POSITION_TILT = 1023 @@ -35,6 +38,13 @@ class Mining(QtCore.QObject): weight_measurement_update_ready__signal = QtCore.pyqtSignal(int) + temp_update_ready__signal = QtCore.pyqtSignal(float) + moisture_update_ready__signal = QtCore.pyqtSignal(float) + loss_tangent_update_ready__signal = QtCore.pyqtSignal(float) + electrical_conductivity_update_ready__signal = QtCore.pyqtSignal(float) + real_dielectric_update_ready__signal = QtCore.pyqtSignal(float) + imaginary_dielectric_update_ready__signal = QtCore.pyqtSignal(float) + def __init__(self, shared_objects): super(Mining, self).__init__() @@ -54,6 +64,13 @@ class Mining(QtCore.QObject): self.mining_transport_move_button = self.left_screen.mining_transport_move_button # type:QtWidgets.QPushButton self.mining_scoop_move_button = self.left_screen.mining_scoop_move_button # type:QtWidgets.QPushButton + self.science_temp_lcd_number = self.left_screen.science_temp_lcd_number # type:QtWidgets.QLCDNumber + self.science_moisture_lcd_number = self.left_screen.science_moisture_lcd_number # type:QtWidgets.QLCDNumber + self.science_loss_tangent_lcd_number = self.left_screen.science_loss_tangent_lcd_number # type:QtWidgets.QLCDNumber + self.science_electrical_conductivity_lcd_number = self.left_screen.science_electrical_conductivity_lcd_number # type:QtWidgets.QLCDNumber + self.science_real_dielectric_lcd_number = self.left_screen.science_real_dielectric_lcd_number # type:QtWidgets.QLCDNumber + self.science_imaginary_dielectric_lcd_number = self.left_screen.science_imaginary_dielectric_lcd_number # type:QtWidgets.QLCDNumber + # ########## Get the settings instance ########## self.settings = QtCore.QSettings() @@ -67,6 +84,8 @@ class Mining(QtCore.QObject): self.mining_status_subscriber = rospy.Subscriber(MINING_STATUS_TOPIC, MiningStatusMessage, self.mining_status_message_received__callback) + self.soil_probe_subscriber = rospy.Subscriber(SOIL_PROBE_TOPIC, SoilSensorStatusMessage, self.on_soil_probe_message_received__callback) + self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1) self.connect_signals_and_slots() @@ -85,6 +104,13 @@ class Mining(QtCore.QObject): self.weight_measurement_update_ready__signal.connect(self.mining_qlcdnumber.display) + self.temp_update_ready__signal.connect(self.science_temp_lcd_number.display) + self.moisture_update_ready__signal.connect(self.science_moisture_lcd_number.display) + self.loss_tangent_update_ready__signal.connect(self.science_loss_tangent_lcd_number.display) + self.electrical_conductivity_update_ready__signal.connect(self.science_electrical_conductivity_lcd_number.display) + self.real_dielectric_update_ready__signal.connect(self.science_real_dielectric_lcd_number.display) + self.imaginary_dielectric_update_ready__signal.connect(self.science_imaginary_dielectric_lcd_number.display) + def on_mining_set_cal_factor_clicked__slot(self): message = MiningControlMessage() @@ -141,3 +167,11 @@ class Mining(QtCore.QObject): self.tilt_position_update_ready__signal.emit(status.tilt_position) self.lift_position_update_ready__signal.emit(status.lift_position) self.weight_measurement_update_ready__signal.emit(status.measured_weight) + + def on_soil_probe_message_received__callback(self, data): + self.temp_update_ready__signal.emit(data.temp_c) + self.moisture_update_ready__signal.emit(data.moisture) + self.loss_tangent_update_ready__signal.emit(data.loss_tangent) + self.electrical_conductivity_update_ready__signal.emit(data.soil_electrical_conductivity) + self.real_dielectric_update_ready__signal.emit(data.real_dielectric_permittivity) + self.imaginary_dielectric_update_ready__signal.emit(data.imaginary_dielectric_permittivity) diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py index 011b6d6..b7e128b 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py @@ -13,7 +13,7 @@ from std_msgs.msg import Float64MultiArray ##################################### # Global Variables ##################################### -RDF_DATA_TOPIC = "/rdf/data" +RDF_DATA_TOPIC = "/rover_science/rdf/data" THREAD_HERTZ = 5 diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 1074bfb..c713d9b 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -1390,7 +1390,7 @@ N/A - 1 + 2 @@ -1652,7 +1652,7 @@ N/A - Mining + Mining / Science @@ -1721,7 +1721,7 @@ N/A - 20 + 16 75 true @@ -1851,13 +1851,13 @@ N/A 300 - 150 + 75 300 - 150 + 75 @@ -1905,7 +1905,7 @@ N/A - + Qt::Vertical @@ -1976,6 +1976,216 @@ N/A + + + + Qt::Horizontal + + + + + + + + + + + + 16 + 75 + true + + + + Science Probe Data + + + Qt::AlignCenter + + + + + + + + + + + + 10 + 75 + true + + + + Temp °C + + + + + + + + 0 + 35 + + + + + + + + + + + + + 10 + 75 + true + + + + Moisture % + + + + + + + + 0 + 35 + + + + + + + + + + + + + 10 + 75 + true + + + + Loss Tangent + + + + + + + + 0 + 35 + + + + + + + + + + + + + 10 + 75 + true + + + + Soil +Electrical +Conductivity + + + + + + + + 0 + 35 + + + + + + + + + + + + + 10 + 75 + true + + + + Real +Dielectric +Permittivity + + + + + + + + 0 + 35 + + + + + + + + + + + + + 10 + 75 + true + + + + Imaginary +Dielectric +Permittivity + + + + + + + + 0 + 35 + + + + + + + + + + + + diff --git a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui index 047115e..f074dd3 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui @@ -1551,7 +1551,7 @@ Position - Mode Readouts + Mode Readouts / Setpoint @@ -1801,6 +1801,34 @@ Position + + + + + + + 10 + 75 + true + + + + Reported Setpoint + + + + + + + + 0 + 50 + + + + + + @@ -2201,63 +2229,52 @@ Position + + + + - - - - - - 14 - 75 - true - - - - Speed Limit - - - Qt::AlignCenter - - - - - - - - 0 - 0 - - - - - 133 - 16777215 - - - - 100 - - - 50 - - - Qt::AlignCenter - - - true - - - Qt::Horizontal - - - false - - - QProgressBar::TopToBottom - - - - + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 14 + 75 + true + + + + Speed Limit + + + Qt::AlignCenter + + + + + + + 100 + + + 50 + + + Qt::Horizontal + + diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py index b9bc1d3..874c5ea 100755 --- a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py @@ -20,8 +20,8 @@ from rover_control.msg import MiningControlMessage, MiningStatusMessage, Gripper NODE_NAME = "effectors_control" # ##### Communication Defines ##### -# DEFAULT_PORT = "/dev/rover/ttyEffectors" -DEFAULT_PORT = "/dev/ttyUSB1" +DEFAULT_PORT = "/dev/rover/ttyEffectors" +# DEFAULT_PORT = "/dev/ttyUSB0" DEFAULT_BAUD = 115200 GRIPPER_NODE_ID = 1 @@ -227,7 +227,7 @@ class EffectorsControl(object): self.process_gripper_control_message() self.send_gripper_status_message() except Exception, error: - print error + pass #print ""error def connect_to_nodes(self): self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id)) @@ -309,29 +309,45 @@ class EffectorsControl(object): def process_gripper_control_message(self): if self.new_gripper_control_message: - if self.gripper_control_message.toggle_light: - self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = not self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] - - self.gripper_registers[GRIPPER_MODBUS_REGISTERS["HOME"]] = self.gripper_control_message.should_home self.gripper_registers[GRIPPER_MODBUS_REGISTERS["MODE"]] = self.gripper_control_message.gripper_mode - gripper_absolute = self.gripper_control_message.gripper_position_absolute - gripper_relative = self.gripper_control_message.gripper_position_relative + if self.gripper_control_message.should_home: + self.gripper_registers[GRIPPER_MODBUS_REGISTERS["HOME"]] = 1 + self.gripper_node.write_registers(0, self.gripper_registers) + self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS) + + homing_complete = False + + while not homing_complete: + registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS)) + # print registers + + if registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0: + homing_complete = True + self.gripper_registers = registers - if -1 < gripper_absolute < UINT16_MAX: - self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(gripper_absolute, 0), GRIPPER_UNIVERSAL_POSITION_MAX) else: - new_position = self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] + gripper_relative - self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(new_position, 0), GRIPPER_UNIVERSAL_POSITION_MAX) + if self.gripper_control_message.toggle_light: + self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = not self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] - self.gripper_node.write_registers(0, self.gripper_registers) + gripper_absolute = self.gripper_control_message.gripper_position_absolute + gripper_relative = self.gripper_control_message.gripper_position_relative - self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS) + if -1 < gripper_absolute < UINT16_MAX: + self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(gripper_absolute, 0), UINT16_MAX) + else: + new_position = self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] + gripper_relative + self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(new_position, 0), UINT16_MAX) + + self.gripper_node.write_registers(0, self.gripper_registers) + + self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS) self.new_gripper_control_message = False def send_gripper_status_message(self): registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS)) + # print registers message = GripperStatusMessage() message.pinch_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_PINCH"]] diff --git a/software/ros_packages/rover_main/launch/rover/science.launch b/software/ros_packages/rover_main/launch/rover/science.launch index 9e88337..1c666e0 100644 --- a/software/ros_packages/rover_main/launch/rover/science.launch +++ b/software/ros_packages/rover_main/launch/rover/science.launch @@ -1,3 +1,5 @@ - + + + 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 7d72530..8016fe8 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 @@ -157,7 +157,8 @@ {name: "/rover_odometry/imu/data", compress: false, rate: 10.0}, {name: "/rover_control/mining/status", compress: false, rate: 5.0}, {name: "/rover_control/gripper/status", compress: false, rate: 5.0}, - {name: "/rdf/data", compress: false, rate: 50.0} + {name: "/rover_science/rdf/data", compress: false, rate: 50.0}, + {name: "/rover_science/soil_probe/data", compress: false, rate: 50.0} ] diff --git a/software/ros_packages/rover_odometry/src/odometry.py b/software/ros_packages/rover_odometry/src/odometry.py index bb3efbf..c1daae6 100755 --- a/software/ros_packages/rover_odometry/src/odometry.py +++ b/software/ros_packages/rover_odometry/src/odometry.py @@ -83,6 +83,7 @@ class Odometry(object): gps = temp.get('gps', None) imu = temp.get('imu', None) + imu_cal = temp.get('imu_cal', None) if gps: # ###### THIS IS HERE TO DEAL WITH UBLOX GPS ##### @@ -98,6 +99,9 @@ class Odometry(object): # print imu self.broadcast_imu(imu) + # if imu_cal: + # print imu_cal + @staticmethod def chksum_nmea(sentence): # String slicing: Grabs all the characters @@ -148,5 +152,6 @@ class Odometry(object): self.imu_data_publisher.publish(message) + if __name__ == "__main__": Odometry() diff --git a/software/ros_packages/rover_science/CMakeLists.txt b/software/ros_packages/rover_science/CMakeLists.txt index f0a18b3..d7a3143 100644 --- a/software/ros_packages/rover_science/CMakeLists.txt +++ b/software/ros_packages/rover_science/CMakeLists.txt @@ -56,6 +56,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES + SoilSensorStatusMessage.msg ) ## Generate services in the 'srv' folder diff --git a/software/ros_packages/rover_science/msg/RDFStatusMessage.msg b/software/ros_packages/rover_science/msg/RDFStatusMessage.msg deleted file mode 100644 index 1b39a87..0000000 --- a/software/ros_packages/rover_science/msg/RDFStatusMessage.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint16 raw_value -double time \ No newline at end of file diff --git a/software/ros_packages/rover_science/msg/SoilSensorStatusMessage.msg b/software/ros_packages/rover_science/msg/SoilSensorStatusMessage.msg new file mode 100644 index 0000000..5e1c02f --- /dev/null +++ b/software/ros_packages/rover_science/msg/SoilSensorStatusMessage.msg @@ -0,0 +1,7 @@ +float32 temp_c +float32 moisture +float32 loss_tangent +float32 soil_electrical_conductivity +float32 real_dielectric_permittivity +float32 imaginary_dielectric_permittivity + diff --git a/software/ros_packages/rover_science/package.xml b/software/ros_packages/rover_science/package.xml index e4bbd2a..72ecb93 100644 --- a/software/ros_packages/rover_science/package.xml +++ b/software/ros_packages/rover_science/package.xml @@ -57,6 +57,7 @@ std_msgs roscpp rospy + message_generation std_msgs diff --git a/software/ros_packages/rover_science/src/rover_science.py b/software/ros_packages/rover_science/src/rover_science.py deleted file mode 100755 index ad49409..0000000 --- a/software/ros_packages/rover_science/src/rover_science.py +++ /dev/null @@ -1,239 +0,0 @@ -#!/usr/bin/env python -##################################### -# Imports -##################################### -# Python native imports -import rospy - -from time import time, sleep - -import serial.rs485 -import minimalmodbus -import numpy - -# Custom Imports -from rover_control.msg import TowerPanTiltControlMessage -from std_msgs.msg import Float64MultiArray - -##################################### -# Global Variables -##################################### -NODE_NAME = "science_node" - -DEFAULT_PORT = "/dev/rover/ttyRDF_SoilProbe" -DEFAULT_BAUD = 9600 - -DEFAULT_INVERT = False - -DEFAULT_RDF_PUBLISHER_TOPIC = "rdf/data" - -PAN_TILT_NODE_ID = 1 - -COMMUNICATIONS_TIMEOUT = 0.1 # Seconds - -RX_DELAY = 0.01 -TX_DELAY = 0.01 - -DEFAULT_HERTZ = 20 - -SOIL_PROBE_ADDRESS = "mar" - -PAN_TILT_MODBUS_REGISTERS = { - "CENTER_ALL": 0, - - "PAN_ADJUST_POSITIVE": 1, - "PAN_ADJUST_NEGATIVE": 2, - "TILT_ADJUST_POSITIVE": 3, - "TILT_ADJUST_NEGATIVE": 4 -} - -SOIL_PROBE_COMMANDS = { - "GET_ADDRESS": "AD=", - "DESCRIPTION": "DS=", - "PROBE_ENABLED": "PE=", - "TAKE_READING": "TR", - "TRANSMIT_READING": "T0", - - "QUERY": "?" -} - -NODE_LAST_SEEN_TIMEOUT = 2 # seconds - - -##################################### -# DriveControl Class Definition -##################################### -class RoverScience(object): - def __init__(self): - rospy.init_node(NODE_NAME) - - self.port = rospy.get_param("~port", DEFAULT_PORT) - self.baud = rospy.get_param("~baud", DEFAULT_BAUD) - - self.pan_tilt_node_id = rospy.get_param("~pan_tilt_node_id", PAN_TILT_NODE_ID) - - self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) - - self.pan_tilt_node = None - self.tower_node = None - - self.connect_to_pan_tilt_and_tower() - - self.rdf_publisher = rospy.Publisher(DEFAULT_RDF_PUBLISHER_TOPIC, Float64MultiArray, queue_size=1) - - self.pan_tilt_control_message = None - self.new_pan_tilt_control_message = False - - self.modbus_nodes_seen_time = time() - - self.data = numpy.array([]) - self.times = numpy.array([]) - self.max_data_len = 200 - - self.count = 0 - self.start_time = time() - - self.run() - - def __setup_minimalmodbus_for_485(self): - self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) # type: serial.Serial - self.pan_tilt_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, - delay_before_rx=RX_DELAY, - delay_before_tx=TX_DELAY) - - def run(self): - # self.send_startup_centering_command() - # self.pan_tilt_node.serial.timeout(500) - - out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["PROBE_ENABLED"] + "1") - self.pan_tilt_node.serial.write(out) - sleep(0.1) - - while not rospy.is_shutdown(): - - out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TAKE_READING"]) - self.pan_tilt_node.serial.write(out) - sleep(0.1) - - out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TRANSMIT_READING"]) - self.pan_tilt_node.serial.write(out) - - start_time = time() - char = "" - response = "" - while char != '\n' and (time() - start_time) < 2: - # print time() - start_time, (time() - start_time) > 2 - if self.pan_tilt_node.serial.inWaiting(): - char = self.pan_tilt_node.serial.read() - response += char - # print char - - if response: - print response - else: - print "timeout" - # print "No response" - - # start_time = time() - # try: - # registers = self.pan_tilt_node.read_registers(0, 1) - # self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()])) - # self.modbus_nodes_seen_time = time() - # except Exception, Error: - # print Error - # - # if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT: - # print "Science not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting." - # return # Exit so respawn can take over - - # time_diff = time() - start_time - - # def run(self): - # # self.send_startup_centering_command() - # while not rospy.is_shutdown(): - # registers = self.pan_tilt_node.read_registers(0, 1) - # self.data = numpy.append(self.data, registers[0]) - # self.times = numpy.append(self.times, time()) - # - # if len(self.data) > self.max_data_len: - # numpy.delete(self.data, 0) - # numpy.delete(self.times, 0) - # - # # for item in self.smoothListTriangle(self.data): - # # print item - # - # print fq.freq_from_fft(self.data, 1/40.0) - # # self.data = self.smoothListGaussian(self.data) - # # - # # w = numpy.fft.fft(self.data) - # # - # # # for item in w: - # # # print abs(item) - # # # - # # # print - # # # print - # # numpy.delete(w, 0) - # # freqs = numpy.fft.fftfreq(len(w), 1/40.0) - # # # # print len(freqs), len(w) - # # # - # # # # print freqs - # # # # print(freqs.min(), freqs.max()) - # # # # # (-0.5, 0.499975) - # # # # - # # # # # Find the peak in the coefficients - # # idx = numpy.argmax(numpy.abs(w)) - # # freq = freqs[idx] - # # freq_in_hertz = abs(freq * 40) - # # print(freq_in_hertz) - - - def smoothListTriangle(self, list, strippedXs=False, degree=5): - - weight = [] - - window = degree * 2 - 1 - - smoothed = [0.0] * (len(list) - window) - - for x in range(1, 2 * degree): weight.append(degree - abs(degree - x)) - - w = numpy.array(weight) - - for i in range(len(smoothed)): - smoothed[i] = sum(numpy.array(list[i:i + window]) * w) / float(sum(w)) - - return smoothed - - def smoothListGaussian(self, list, strippedXs=False, degree=5): - - window = degree * 2 - 1 - - weight = numpy.array([1.0] * window) - - weightGauss = [] - - for i in range(window): - i = i - degree + 1 - - frac = i / float(window) - - gauss = 1 / (numpy.exp((4 * (frac)) ** 2)) - - weightGauss.append(gauss) - - weight = numpy.array(weightGauss) * weight - - smoothed = [0.0] * (len(list) - window) - - for i in range(len(smoothed)): - smoothed[i] = sum(numpy.array(list[i:i + window]) * weight) / sum(weight) - - return smoothed - - def connect_to_pan_tilt_and_tower(self): - self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.pan_tilt_node_id)) - self.__setup_minimalmodbus_for_485() - - -if __name__ == "__main__": - RoverScience() diff --git a/software/ros_packages/rover_science/src/rover_science_node.py b/software/ros_packages/rover_science/src/rover_science_node.py new file mode 100755 index 0000000..a3c448f --- /dev/null +++ b/software/ros_packages/rover_science/src/rover_science_node.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python +##################################### +# Imports +##################################### +# Python native imports +import rospy + +from time import time, sleep + +import serial.rs485 +import minimalmodbus +import numpy + +# Custom Imports +from rover_science.msg import SoilSensorStatusMessage +from std_msgs.msg import Float64MultiArray + +##################################### +# Global Variables +##################################### +NODE_NAME = "science_node" + +DEFAULT_PORT = "/dev/rover/ttyRDF_SoilProbe" +DEFAULT_RDF_BAUD = 115200 +DEFAULT_SOIL_BAUD = 9600 + +DEFAULT_RDF_PUBLISHER_TOPIC = "rdf/data" +DEFAULT_SOIL_PROBE_PUBLISHER_TOPIC = "soil_probe/data" + +RDF_NODE_ID = 1 + +COMMUNICATIONS_TIMEOUT = 0.1 # Seconds + +RX_DELAY = 0.01 +TX_DELAY = 0.01 + +DEFAULT_HERTZ = 20 +FAILED_RDF_LIMIT = 20 + +SOIL_PROBE_READ_TIMEOUT = 8 +SOIL_PROBE_EXIT_TIMEOUT = 20 + +SOIL_PROBE_ADDRESS = "mar" + +PAN_TILT_MODBUS_REGISTERS = { + "CENTER_ALL": 0, + + "PAN_ADJUST_POSITIVE": 1, + "PAN_ADJUST_NEGATIVE": 2, + "TILT_ADJUST_POSITIVE": 3, + "TILT_ADJUST_NEGATIVE": 4 +} + +SOIL_PROBE_COMMANDS = { + "GET_ADDRESS": "AD=", + "DESCRIPTION": "DS=", + "PROBE_ENABLED": "PE=", + "TAKE_READING": "TR", + "TRANSMIT_READING": "T3", + + "QUERY": "?" +} + +TRANSMIT_SET_3_INDICES = { + "TEMP C": 0, + "Moisture": 2, + "Loss Tangent": 3, + "Soil Electrical Conductivity (tc)": 4, + "Real Dielectric Permittivity (tc)": 6, + "Imag Dielectric Permittivity (tc)": 8, +} + +NODE_LAST_SEEN_TIMEOUT = 2 # seconds + + +##################################### +# DriveControl Class Definition +##################################### +class RoverScience(object): + INSTRUMENTS = [ + "RDF", + "SOIL" + ] + + def __init__(self): + rospy.init_node(NODE_NAME) + + self.port = rospy.get_param("~port", DEFAULT_PORT) + self.baud = rospy.get_param("~baud", DEFAULT_RDF_BAUD) + + self.science_node_id = rospy.get_param("~pan_tilt_node_id", RDF_NODE_ID) + + self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) + + self.rdf_node = None + self.soil_node = None + + self.connect_to_rdf() + + self.rdf_publisher = rospy.Publisher(DEFAULT_RDF_PUBLISHER_TOPIC, Float64MultiArray, queue_size=1) + self.soil_probe_publisher = rospy.Publisher(DEFAULT_SOIL_PROBE_PUBLISHER_TOPIC, SoilSensorStatusMessage, queue_size=1) + + self.modbus_nodes_seen_time = time() + + self.failed_rdf_modbus_count = 0 + + self.soil_probe_timeout_cumulative = 0 + + self.which_instrument = self.INSTRUMENTS.index("RDF") + + self.probe_response_line = "" + + self.run() + + def __setup_minimalmodbus_for_485(self): + self.rdf_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) # type: serial.Serial + self.rdf_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, + delay_before_rx=RX_DELAY, + delay_before_tx=TX_DELAY) + + def run(self): + while not rospy.is_shutdown(): + if self.which_instrument == self.INSTRUMENTS.index("RDF"): + try: + registers = self.rdf_node.read_registers(0, 1) + self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()])) + self.failed_rdf_modbus_count = 0 + except Exception, e: + # print e + self.failed_rdf_modbus_count += 1 + + if self.failed_rdf_modbus_count == FAILED_RDF_LIMIT: + print "RDF not present. Trying soil sensor" + self.which_instrument = self.INSTRUMENTS.index("SOIL") + + elif self.which_instrument == self.INSTRUMENTS.index("SOIL"): + if not self.soil_node: + self.switch_node_to_soil_probe() + + self.broadcast_soil_sensor_data() + + if self.soil_probe_timeout_cumulative > SOIL_PROBE_EXIT_TIMEOUT: + print "No science devices present. Exiting..." + return + + def switch_node_to_soil_probe(self): + del self.rdf_node + + self.soil_node = serial.rs485.RS485(self.port, baudrate=DEFAULT_SOIL_BAUD, timeout=COMMUNICATIONS_TIMEOUT) # type: serial.Serial + self.soil_node.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, delay_before_rx=RX_DELAY, delay_before_tx=TX_DELAY) + + out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["PROBE_ENABLED"] + "1") + self.soil_node.write(out) + sleep(0.1) + + def broadcast_soil_sensor_data(self): + self.request_soil_reading() + self.request_reading_results() + self.get_probe_response_line() + self.process_probe_response_and_send() + + def process_probe_response_and_send(self): + if self.probe_response_line != "": + + try: + self.probe_response_line = self.probe_response_line.replace(SOIL_PROBE_ADDRESS, "") + + split_results = self.probe_response_line.split(",") + + temp_c = split_results[TRANSMIT_SET_3_INDICES["TEMP C"]] + moisture = split_results[TRANSMIT_SET_3_INDICES["Moisture"]] + loss_tangent = split_results[TRANSMIT_SET_3_INDICES["Loss Tangent"]] + sec_tc = split_results[TRANSMIT_SET_3_INDICES["Soil Electrical Conductivity (tc)"]] + rdp_tc = split_results[TRANSMIT_SET_3_INDICES["Real Dielectric Permittivity (tc)"]] + idp_tc = split_results[TRANSMIT_SET_3_INDICES["Imag Dielectric Permittivity (tc)"]] + + message = SoilSensorStatusMessage() + message.temp_c = float(temp_c) + message.moisture = float(moisture) + message.loss_tangent = float(loss_tangent) + message.soil_electrical_conductivity = float(sec_tc) + message.real_dielectric_permittivity = float(rdp_tc) + message.imaginary_dielectric_permittivity = float(idp_tc) + + self.soil_probe_publisher.publish(message) + + except: + print "Soil probe line corrupted. Trying again..." + + def get_probe_response_line(self): + start_time = time() + char = "" + self.probe_response_line = "" + + while char != '\n' and (time() - start_time) < 2: + if self.soil_node.inWaiting(): + char = self.soil_node.read() + self.probe_response_line += char + + if self.probe_response_line: + self.soil_probe_timeout_cumulative = 0 + # print self.probe_response_line + else: + # print "timeout" + self.soil_probe_timeout_cumulative += 2 + + def request_soil_reading(self): + out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TAKE_READING"]) + self.soil_node.write(out) + sleep(0.1) + + def request_reading_results(self): + out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TRANSMIT_READING"]) + self.soil_node.write(out) + + def smoothListTriangle(self, list, strippedXs=False, degree=5): + + weight = [] + + window = degree * 2 - 1 + + smoothed = [0.0] * (len(list) - window) + + for x in range(1, 2 * degree): weight.append(degree - abs(degree - x)) + + w = numpy.array(weight) + + for i in range(len(smoothed)): + smoothed[i] = sum(numpy.array(list[i:i + window]) * w) / float(sum(w)) + + return smoothed + + def smoothListGaussian(self, list, strippedXs=False, degree=5): + + window = degree * 2 - 1 + + weight = numpy.array([1.0] * window) + + weightGauss = [] + + for i in range(window): + i = i - degree + 1 + + frac = i / float(window) + + gauss = 1 / (numpy.exp((4 * (frac)) ** 2)) + + weightGauss.append(gauss) + + weight = numpy.array(weightGauss) * weight + + smoothed = [0.0] * (len(list) - window) + + for i in range(len(smoothed)): + smoothed[i] = sum(numpy.array(list[i:i + window]) * weight) / sum(weight) + + return smoothed + + def connect_to_rdf(self): + self.rdf_node = minimalmodbus.Instrument(self.port, int(self.science_node_id)) + self.__setup_minimalmodbus_for_485() + + +if __name__ == "__main__": + RoverScience() diff --git a/software/rover_setup.sh b/software/rover_setup.sh index 0845072..94f9ac9 100755 --- a/software/rover_setup.sh +++ b/software/rover_setup.sh @@ -11,7 +11,6 @@ folders_to_link=( rover_control rover_arm rover_status - zed_wrapper nimbro_topic_transport rover_main rover_odometry