From a4f04eda7e4b499c5f2b9b2445a72f465d0dea05 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Sun, 12 Aug 2018 01:25:20 -0700 Subject: [PATCH] Updated mining readouts for addition of science cam controls. Working science cam controls. Fixed missing soil probe transports. --- software/firmware/mining/mining.ino | 41 +- .../src/Framework/MiscSystems/MiningCore.py | 122 ++-- .../src/Resources/Ui/left_screen.ui | 624 ++++++++++-------- .../msg/CameraControlMessage.msg | 1 - .../effectors_control/effectors_control.py | 20 +- .../src/effectors_control/effectors_tester.py | 10 +- .../src/effectors_control/emergency_scale.py | 1 - .../rover/topic_transport_senders.launch | 1 + 8 files changed, 466 insertions(+), 354 deletions(-) diff --git a/software/firmware/mining/mining.ino b/software/firmware/mining/mining.ino index 562721e..65c0b4e 100644 --- a/software/firmware/mining/mining.ino +++ b/software/firmware/mining/mining.ino @@ -1,6 +1,7 @@ ////////// Includes ////////// #include "HX711.h" #include +#include ////////// Hardware / Data Enumerations ////////// enum HARDWARE { @@ -65,13 +66,12 @@ enum MODBUS_REGISTERS { ZOOM_OUT = 11, FULL_ZOOM_IN = 12, FULL_ZOOM_OUT = 13, - FOCUS = 14, - SHOOT = 15, + SHOOT = 14, // Outputs - CURRENT_POSITION_LIFT = 16, - CURRENT_POSITION_TILT = 17, - MEASURED_WEIGHT = 18 + CURRENT_POSITION_LIFT = 15, + CURRENT_POSITION_TILT = 16, + MEASURED_WEIGHT = 17 }; enum CAMERA_VIEW_MODES { @@ -92,7 +92,7 @@ float last_calibration_factor = -120000; //for the scale // modbus stuff const uint8_t node_id = 2; const uint8_t mobus_serial_port_number = 2; -uint16_t modbus_data[] = {0, 0, 0, 0, 9999, 9999, 0, 0, 895, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; +uint16_t modbus_data[] = {0, 0, 0, 0, 9999, 9999, 0, 0, 895, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t num_modbus_registers = 0; int8_t poll_state = 0; bool communication_good = false; @@ -107,10 +107,11 @@ uint8_t message_count = 0; ////////// Class Instantiations ////////// HX711 scale(HARDWARE::SCALE_DOUT, HARDWARE::SCALE_CLK); Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN); +camera science_camera(HARDWARE::CAMERA_CONTROL); void setup() { Serial.begin(9600); // debug - while(!Serial); + // while(!Serial); setup_hardware(); num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); @@ -291,33 +292,23 @@ void apply_camera_commands(){ ////// Zoom controls, process all, even if conflicting /////// if(modbus_data[MODBUS_REGISTERS::ZOOM_IN]){ - Serial.println("ZOOM IN"); + science_camera.slowZoomIn(); modbus_data[MODBUS_REGISTERS::ZOOM_IN] = 0; }else if(modbus_data[MODBUS_REGISTERS::ZOOM_OUT]){ - Serial.println("ZOOM OUT"); + science_camera.slowZoomOut(); modbus_data[MODBUS_REGISTERS::ZOOM_OUT] = 0; }else if(modbus_data[MODBUS_REGISTERS::FULL_ZOOM_IN]){ - Serial.println("FULL ZOOM IN"); + science_camera.fullZoomIn(); modbus_data[MODBUS_REGISTERS::FULL_ZOOM_IN] = 0; - } - - if(modbus_data[MODBUS_REGISTERS::FULL_ZOOM_OUT]){ - Serial.println("FULL ZOOM OUT"); + }else if(modbus_data[MODBUS_REGISTERS::FULL_ZOOM_OUT]){ + science_camera.fullZoomOut(); modbus_data[MODBUS_REGISTERS::FULL_ZOOM_OUT] = 0; - } - - ////// Focus and shoot controls /////// - if(modbus_data[MODBUS_REGISTERS::FOCUS]){ - Serial.println("FOCUS"); - modbus_data[MODBUS_REGISTERS::FOCUS] = 0; - } - - if(modbus_data[MODBUS_REGISTERS::SHOOT]){ - Serial.println("SHOOT"); + } else if(modbus_data[MODBUS_REGISTERS::SHOOT]){ + science_camera.shoot(); modbus_data[MODBUS_REGISTERS::SHOOT] = 0; } ///// camera process here - + science_camera.update(); } //---Set Motor Output---// 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 0f13075..56618bc 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py @@ -32,6 +32,15 @@ MEASURE_POSITION_TILT = 1023 SCOOP_POSITION_LIFT = 228 SCOOP_POSITION_TILT = 215 +PANORAMA_POSITION_LIFT = 535 +PANORAMA_POSITION_TILT = 995 + +SAMPLE_POSITION_LIFT = 220 +SAMPLE_POSITION_TILT = 240 + +PROBE_POSITION_LIFT = 950 +PROBE_POSITION_TILT = 440 + ##################################### # UbiquitiRadioSettings Class Definition @@ -58,10 +67,7 @@ class Mining(QtCore.QObject): self.left_screen = self.shared_objects["screens"]["left_screen"] self.mining_qlcdnumber = self.left_screen.mining_qlcdnumber # type:QtWidgets.QLCDNumber - self.mining_tare_button = self.left_screen.mining_tare_button # type:QtWidgets.QPushButton - self.mining_measure_button = self.left_screen.mining_measure_button # type:QtWidgets.QPushButton - self.mining_cal_factor_spinbox = self.left_screen.mining_cal_factor_spinbox # type:QtWidgets.QSpinBox - self.mining_set_cal_factor_button = self.left_screen.mining_set_cal_factor_button # type:QtWidgets.QPushButton + self.mining_zero_button = self.left_screen.mining_zero_button # type:QtWidgets.QPushButton self.lift_position_progress_bar = self.left_screen.lift_position_progress_bar # type:QtWidgets.QProgressBar self.tilt_position_progress_bar = self.left_screen.tilt_position_progress_bar # type:QtWidgets.QProgressBar @@ -69,6 +75,10 @@ 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.mining_panorama_button = self.left_screen.mining_panorama_button # type:QtWidgets.QPushButton + self.mining_sample_button = self.left_screen.mining_sample_button # type:QtWidgets.QPushButton + self.mining_probe_button = self.left_screen.mining_probe_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 @@ -81,11 +91,10 @@ class Mining(QtCore.QObject): self.cam_zoom_in_button = self.left_screen.cam_zoom_in_button # type:QtWidgets.QPushButton self.cam_zoom_out_button = self.left_screen.cam_zoom_out_button # type:QtWidgets.QPushButton - self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton - self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton + self.cam_full_zoom_in_button = self.left_screen.cam_full_zoom_in_button # type:QtWidgets.QPushButton + self.cam_full_zoom_out_button = self.left_screen.cam_full_zoom_out_button # type:QtWidgets.QPushButton - self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton - self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton + self.cam_shoot_button = self.left_screen.cam_shoot_button # type:QtWidgets.QPushButton # ########## Get the settings instance ########## self.settings = QtCore.QSettings() @@ -106,17 +115,22 @@ class Mining(QtCore.QObject): self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1) self.camera_control_publisher = rospy.Publisher(CAMERA_CONTROL_TOPIC, CameraControlMessage, queue_size=1) + self.current_scale_measurement = 0 + self.scale_zero_offset = 0 + self.connect_signals_and_slots() def connect_signals_and_slots(self): - self.mining_set_cal_factor_button.clicked.connect(self.on_mining_set_cal_factor_clicked__slot) - self.mining_tare_button.clicked.connect(self.on_mining_tare_clicked__slot) - self.mining_measure_button.clicked.connect(self.on_mining_measure_clicked__slot) + self.mining_zero_button.clicked.connect(self.on_mining_zero_clicked__slot) self.mining_measure_move_button.clicked.connect(self.on_mining_move_measure_clicked__slot) self.mining_transport_move_button.clicked.connect(self.on_mining_move_transport_clicked__slot) self.mining_scoop_move_button.clicked.connect(self.on_mining_move_scoop_clicked__slot) + self.mining_panorama_button.clicked.connect(self.on_mining_move_panorama_clicked__slot) + self.mining_sample_button.clicked.connect(self.on_mining_move_sample_clicked__slot) + self.mining_probe_button.clicked.connect(self.on_mining_move_probe_clicked__slot) + 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) @@ -132,32 +146,14 @@ class Mining(QtCore.QObject): self.cam_lcd_output_button.clicked.connect(self.on_cam_lcd_button_clicked__slot) self.cam_network_output_button.clicked.connect(self.on_cam_network_button_clicked__slot) - def on_mining_set_cal_factor_clicked__slot(self): - message = MiningControlMessage() + self.cam_zoom_in_button.clicked.connect(self.on_cam_zoom_in_button_clicked__slot) + self.cam_zoom_out_button.clicked.connect(self.on_cam_zoom_out_button_clicked__slot) + self.cam_full_zoom_in_button.clicked.connect(self.on_cam_full_zoom_in_button_clicked__slot) + self.cam_full_zoom_out_button.clicked.connect(self.on_cam_full_zoom_out_button_clicked__slot) + self.cam_shoot_button.clicked.connect(self.on_cam_shoot_button_clicked__slot) - message.tilt_set_absolute = 1024 - message.lift_set_absolute = 1024 - message.cal_factor = self.mining_cal_factor_spinbox.value() - - self.mining_control_publisher.publish(message) - - def on_mining_tare_clicked__slot(self): - message = MiningControlMessage() - message.tilt_set_absolute = 1024 - message.lift_set_absolute = 1024 - message.cal_factor = -1 - message.tare = 1 - - self.mining_control_publisher.publish(message) - - def on_mining_measure_clicked__slot(self): - message = MiningControlMessage() - message.tilt_set_absolute = 1024 - message.lift_set_absolute = 1024 - message.cal_factor = -1 - message.measure = True - - self.mining_control_publisher.publish(message) + def on_mining_zero_clicked__slot(self): + self.scale_zero_offset = -self.current_scale_measurement def on_mining_move_transport_clicked__slot(self): message = MiningControlMessage() @@ -183,6 +179,30 @@ class Mining(QtCore.QObject): self.mining_control_publisher.publish(message) + def on_mining_move_panorama_clicked__slot(self): + message = MiningControlMessage() + message.tilt_set_absolute = PANORAMA_POSITION_TILT + message.lift_set_absolute = PANORAMA_POSITION_LIFT + message.cal_factor = -1 + + self.mining_control_publisher.publish(message) + + def on_mining_move_sample_clicked__slot(self): + message = MiningControlMessage() + message.tilt_set_absolute = SAMPLE_POSITION_TILT + message.lift_set_absolute = SAMPLE_POSITION_LIFT + message.cal_factor = -1 + + self.mining_control_publisher.publish(message) + + def on_mining_move_probe_clicked__slot(self): + message = MiningControlMessage() + message.tilt_set_absolute = PROBE_POSITION_TILT + message.lift_set_absolute = PROBE_POSITION_LIFT + message.cal_factor = -1 + + self.mining_control_publisher.publish(message) + def on_cam_lcd_button_clicked__slot(self): message = CameraControlMessage() message.camera_mode = 1 @@ -193,11 +213,35 @@ class Mining(QtCore.QObject): message.camera_mode = 2 self.camera_control_publisher.publish(message) + def on_cam_zoom_in_button_clicked__slot(self): + message = CameraControlMessage() + message.zoom_in = 1 + self.camera_control_publisher.publish(message) + + def on_cam_zoom_out_button_clicked__slot(self): + message = CameraControlMessage() + message.zoom_out = 1 + self.camera_control_publisher.publish(message) + + def on_cam_full_zoom_in_button_clicked__slot(self): + message = CameraControlMessage() + message.full_zoom_in = 1 + self.camera_control_publisher.publish(message) + + def on_cam_full_zoom_out_button_clicked__slot(self): + message = CameraControlMessage() + message.full_zoom_out = 1 + self.camera_control_publisher.publish(message) + + def on_cam_shoot_button_clicked__slot(self): + message = CameraControlMessage() + message.shoot = 1 + self.camera_control_publisher.publish(message) + def mining_status_message_received__callback(self, status): 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.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) @@ -208,4 +252,6 @@ class Mining(QtCore.QObject): self.imaginary_dielectric_update_ready__signal.emit(data.imaginary_dielectric_permittivity) def on_scale_measurement_received__callback(self, data): - self.weight_measurement_update_ready__signal.emit(data.data * 1000) \ No newline at end of file + grams = data.data * 1000 + self.current_scale_measurement = grams + self.weight_measurement_update_ready__signal.emit(grams + self.scale_zero_offset) 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 e6d9ec6..4b4ef21 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,145 +1390,8 @@ N/A - 0 + 2 - - - Science Cam - - - - - - - - - 75 - true - - - - Video Output - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - LCD - - - - - - - Network - - - - - - - - - - - Qt::Horizontal - - - - 422 - 20 - - - - - - - - - - - 75 - true - - - - Controls - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - Zoom In - - - - - - - Zoom Out - - - - - - - Full Zoom In - - - - - - - Full Zoom Out - - - - - - - Focus - - - - - - - Shoot - - - - - - - - - - - Qt::Vertical - - - - 20 - 331 - - - - - - RDF @@ -2017,7 +1880,14 @@ N/A Mining / Science - + + + + Qt::Horizontal + + + + @@ -2048,7 +1918,7 @@ N/A - + @@ -2083,7 +1953,7 @@ N/A - 16 + 14 75 true @@ -2092,101 +1962,10 @@ N/A Scoop Measurement - Qt::AlignCenter + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Tare - - - - - - - Measure - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - 65535 - - - 114 - - - - - - - Set Cal Factor - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - @@ -2213,13 +1992,13 @@ N/A 300 - 75 + 45 300 - 75 + 45 @@ -2242,7 +2021,7 @@ N/A - 19 + 12 75 true @@ -2265,6 +2044,13 @@ N/A + + + + Zero + + + @@ -2280,27 +2066,20 @@ N/A - - - - Qt::Horizontal - - - - + - - - Qt::Horizontal + + + + 75 + true + - - - 40 - 20 - + + Mining - + @@ -2324,7 +2103,7 @@ N/A - + Qt::Horizontal @@ -2336,16 +2115,50 @@ N/A + + + + + 75 + true + + + + Science + + + + + + + Panaroma + + + + + + + Sample + + + + + + + Probe + + + - + Qt::Horizontal - + @@ -2353,7 +2166,7 @@ N/A - 16 + 14 75 true @@ -2362,7 +2175,7 @@ N/A Science Probe Data - Qt::AlignCenter + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter @@ -2374,7 +2187,7 @@ N/A - 10 + 9 75 true @@ -2389,7 +2202,13 @@ N/A 0 - 35 + 30 + + + + + 16777215 + 30 @@ -2402,7 +2221,7 @@ N/A - 10 + 9 75 true @@ -2417,7 +2236,13 @@ N/A 0 - 35 + 30 + + + + + 16777215 + 30 @@ -2430,7 +2255,7 @@ N/A - 10 + 9 75 true @@ -2445,7 +2270,13 @@ N/A 0 - 35 + 30 + + + + + 16777215 + 30 @@ -2458,15 +2289,14 @@ N/A - 10 + 9 75 true Soil -Electrical -Conductivity +Electrical Conductivity @@ -2475,7 +2305,13 @@ Conductivity 0 - 35 + 30 + + + + + 16777215 + 30 @@ -2488,15 +2324,14 @@ Conductivity - 10 + 9 75 true Real -Dielectric -Permittivity +Dielectric Permittivity @@ -2505,7 +2340,13 @@ Permittivity 0 - 35 + 30 + + + + + 16777215 + 30 @@ -2518,15 +2359,14 @@ Permittivity - 10 + 9 75 true Imaginary -Dielectric -Permittivity +Dielectric Permittivity @@ -2535,7 +2375,13 @@ Permittivity 0 - 35 + 30 + + + + + 16777215 + 30 @@ -2548,6 +2394,232 @@ Permittivity + + + + Qt::Horizontal + + + + + + + + + + 14 + 75 + true + + + + Camera Controls + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + + + + + + + 75 + true + + + + Video Output + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + + + 95 + 0 + + + + Network + + + + + + + + 95 + 0 + + + + LCD + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + + 75 + true + + + + Controls + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + + + + + + + 95 + 0 + + + + Zoom In + + + + + + + + 95 + 0 + + + + Full Zoom In + + + + + + + + + + + + 95 + 0 + + + + Zoom Out + + + + + + + + 95 + 0 + + + + Full Zoom Out + + + + + + + + + + 95 + 50 + + + + Shoot + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + diff --git a/software/ros_packages/rover_control/msg/CameraControlMessage.msg b/software/ros_packages/rover_control/msg/CameraControlMessage.msg index ad45e94..063ab35 100644 --- a/software/ros_packages/rover_control/msg/CameraControlMessage.msg +++ b/software/ros_packages/rover_control/msg/CameraControlMessage.msg @@ -4,5 +4,4 @@ bool zoom_in bool zoom_out bool full_zoom_in bool full_zoom_out -bool focus bool shoot \ No newline at end of file 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 f7467df..1c11ee7 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 @@ -101,12 +101,11 @@ MINING_MODBUS_REGISTERS = { "ZOOM_OUT": 11, "FULL_ZOOM_IN": 12, "FULL_ZOOM_OUT": 13, - "FOCUS": 14, - "SHOOT": 15, + "SHOOT": 14, - "CURRENT_POSITION_LIFT": 16, - "CURRENT_POSITION_TILT": 17, - "MEASURED_WEIGHT": 18 + "CURRENT_POSITION_LIFT": 15, + "CURRENT_POSITION_TILT": 16, + "MEASURED_WEIGHT": 17 } @@ -300,12 +299,11 @@ class EffectorsControl(object): self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 0 self.mining_registers[MINING_MODBUS_REGISTERS["CHANGE_VIEW_MODE"]] = self.camera_control_message.camera_mode - self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_IN"]] = 0 - self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_OUT"]] = 0 - self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_IN"]] = 0 - self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_OUT"]] = 0 - self.mining_registers[MINING_MODBUS_REGISTERS["FOCUS"]] = 0 - self.mining_registers[MINING_MODBUS_REGISTERS["SHOOT"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_IN"]] = self.camera_control_message.zoom_in + self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_OUT"]] = self.camera_control_message.zoom_out + self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_IN"]] = self.camera_control_message.full_zoom_in + self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_OUT"]] = self.camera_control_message.full_zoom_out + self.mining_registers[MINING_MODBUS_REGISTERS["SHOOT"]] = self.camera_control_message.shoot self.mining_node.write_registers(0, self.mining_registers) self.modbus_nodes_seen_time = time() diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py b/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py index 2d066da..d57fdc5 100755 --- a/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py @@ -12,11 +12,17 @@ publisher = rospy.Publisher(TOPIC, CameraControlMessage, queue_size=1) time.sleep(2) -message = CameraControlMessage() -message.camera_mode = 2 +# message = CameraControlMessage() +# message.full_zoom_in = 1 +# publisher.publish(message) +time.sleep(2) + +message = CameraControlMessage() +message.shoot = 1 publisher.publish(message) + time.sleep(5) # message = MiningControlMessage() diff --git a/software/ros_packages/rover_control/src/effectors_control/emergency_scale.py b/software/ros_packages/rover_control/src/effectors_control/emergency_scale.py index 13ae3fb..8fb5c85 100644 --- a/software/ros_packages/rover_control/src/effectors_control/emergency_scale.py +++ b/software/ros_packages/rover_control/src/effectors_control/emergency_scale.py @@ -49,7 +49,6 @@ class EmergencyScale(object): message = Float64() message.data = float(value) self.publisher.publish(message) - print except: print "No data" 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 c8a4d40..9721b6e 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,6 +157,7 @@ {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: "/rover_science/soil_probe/data", compress: false, rate: 5.0}, {name: "/rover_science/rdf/data", compress: false, rate: 50.0}, {name: "/rover_control/scale/measurement", compress: false, rate: 20.0} ]