Gripper control changed a bit, needs more work. Readouts for soil probe on gui, finished soil/rdf node.

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) */
+    JsonObject& imu_cal_object = root.createNestedObject("imu_cal");
-  /* 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);
 
-  /* The data should be ignored until the system calibration is > 0 */
+    imu_cal_object["gyro"] = gyro;
-  Serial.print("\t");
+    imu_cal_object["accel"] = accel;
-  if (!system)
+    imu_cal_object["mag"] = mag;
-  {
-    Serial.print("! ");
-  }
-
-  /* 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);
 }
 
 void process_gps_and_send_if_ready(JsonObject &root) {

software/ground_station_setup.sh

@@ -14,6 +14,7 @@ folders_to_link=(
     rover_camera
     rover_status
     rover_arm
+    rover_science
 )
 
 # Print heading

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 = ""

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)
+        left_y_axis = speed_limit * (-(left_y_axis - STICK_OFFSET) / STICK_MAX)
-        right_y_axis = throttle_axis * (-(right_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)

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,6 +336,12 @@ class XBOXControllerControlSender(QtCore.QThread):
             for signal in signal_map["UNSET"]:
                 signal.emit(COLOR_NONE)
 
+            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(

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)

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
 

software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui

@@ -1390,7 +1390,7 @@ N/A</string>
          </size>
         </property>
         <property name="currentIndex">
-         <number>1</number>
+         <number>2</number>
         </property>
         <widget class="QWidget" name="tab_5">
          <attribute name="title">
@@ -1652,7 +1652,7 @@ N/A</string>
         </widget>
         <widget class="QWidget" name="tab_3">
          <attribute name="title">
-          <string>Mining</string>
+          <string>Mining / Science</string>
          </attribute>
          <layout class="QGridLayout" name="gridLayout_8">
           <item row="5" column="0">
@@ -1721,7 +1721,7 @@ N/A</string>
            <widget class="QLabel" name="label_13">
             <property name="font">
              <font>
-              <pointsize>20</pointsize>
+              <pointsize>16</pointsize>
               <weight>75</weight>
               <bold>true</bold>
              </font>
@@ -1851,13 +1851,13 @@ N/A</string>
               <property name="minimumSize">
                <size>
                 <width>300</width>
-                <height>150</height>
+                <height>75</height>
                </size>
               </property>
               <property name="maximumSize">
                <size>
                 <width>300</width>
-                <height>150</height>
+                <height>75</height>
                </size>
               </property>
               <property name="font">
@@ -1905,7 +1905,7 @@ N/A</string>
             </item>
            </layout>
           </item>
-          <item row="8" column="0">
+          <item row="10" column="0">
            <spacer name="verticalSpacer">
             <property name="orientation">
              <enum>Qt::Vertical</enum>
@@ -1976,6 +1976,216 @@ N/A</string>
             </item>
            </layout>
           </item>
+          <item row="8" column="0">
+           <widget class="Line" name="line_9">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="0">
+           <layout class="QHBoxLayout" name="horizontalLayout_16">
+            <item>
+             <layout class="QVBoxLayout" name="verticalLayout_10">
+              <item>
+               <widget class="QLabel" name="label_20">
+                <property name="font">
+                 <font>
+                  <pointsize>16</pointsize>
+                  <weight>75</weight>
+                  <bold>true</bold>
+                 </font>
+                </property>
+                <property name="text">
+                 <string>Science Probe Data</string>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignCenter</set>
+                </property>
+               </widget>
+              </item>
+              <item>
+               <layout class="QGridLayout" name="gridLayout_17">
+                <item row="0" column="0">
+                 <layout class="QVBoxLayout" name="verticalLayout_6">
+                  <item>
+                   <widget class="QLabel" name="label_21">
+                    <property name="font">
+                     <font>
+                      <pointsize>10</pointsize>
+                      <weight>75</weight>
+                      <bold>true</bold>
+                     </font>
+                    </property>
+                    <property name="text">
+                     <string>Temp °C</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLCDNumber" name="science_temp_lcd_number">
+                    <property name="minimumSize">
+                     <size>
+                      <width>0</width>
+                      <height>35</height>
+                     </size>
+                    </property>
+                   </widget>
+                  </item>
+                 </layout>
+                </item>
+                <item row="0" column="1">
+                 <layout class="QVBoxLayout" name="verticalLayout_11">
+                  <item>
+                   <widget class="QLabel" name="moisture_label">
+                    <property name="font">
+                     <font>
+                      <pointsize>10</pointsize>
+                      <weight>75</weight>
+                      <bold>true</bold>
+                     </font>
+                    </property>
+                    <property name="text">
+                     <string>Moisture %</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLCDNumber" name="science_moisture_lcd_number">
+                    <property name="minimumSize">
+                     <size>
+                      <width>0</width>
+                      <height>35</height>
+                     </size>
+                    </property>
+                   </widget>
+                  </item>
+                 </layout>
+                </item>
+                <item row="0" column="2">
+                 <layout class="QVBoxLayout" name="verticalLayout_12">
+                  <item>
+                   <widget class="QLabel" name="tangent_label">
+                    <property name="font">
+                     <font>
+                      <pointsize>10</pointsize>
+                      <weight>75</weight>
+                      <bold>true</bold>
+                     </font>
+                    </property>
+                    <property name="text">
+                     <string>Loss Tangent</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLCDNumber" name="science_loss_tangent_lcd_number">
+                    <property name="minimumSize">
+                     <size>
+                      <width>0</width>
+                      <height>35</height>
+                     </size>
+                    </property>
+                   </widget>
+                  </item>
+                 </layout>
+                </item>
+                <item row="1" column="0">
+                 <layout class="QVBoxLayout" name="verticalLayout_13">
+                  <item>
+                   <widget class="QLabel" name="conductivity_label">
+                    <property name="font">
+                     <font>
+                      <pointsize>10</pointsize>
+                      <weight>75</weight>
+                      <bold>true</bold>
+                     </font>
+                    </property>
+                    <property name="text">
+                     <string>Soil
+Electrical
+Conductivity</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLCDNumber" name="science_electrical_conductivity_lcd_number">
+                    <property name="minimumSize">
+                     <size>
+                      <width>0</width>
+                      <height>35</height>
+                     </size>
+                    </property>
+                   </widget>
+                  </item>
+                 </layout>
+                </item>
+                <item row="1" column="1">
+                 <layout class="QVBoxLayout" name="verticalLayout_16">
+                  <item>
+                   <widget class="QLabel" name="real_dielectric_label">
+                    <property name="font">
+                     <font>
+                      <pointsize>10</pointsize>
+                      <weight>75</weight>
+                      <bold>true</bold>
+                     </font>
+                    </property>
+                    <property name="text">
+                     <string>Real
+Dielectric
+Permittivity</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLCDNumber" name="science_real_dielectric_lcd_number">
+                    <property name="minimumSize">
+                     <size>
+                      <width>0</width>
+                      <height>35</height>
+                     </size>
+                    </property>
+                   </widget>
+                  </item>
+                 </layout>
+                </item>
+                <item row="1" column="2">
+                 <layout class="QVBoxLayout" name="verticalLayout_18">
+                  <item>
+                   <widget class="QLabel" name="imaginary_label">
+                    <property name="font">
+                     <font>
+                      <pointsize>10</pointsize>
+                      <weight>75</weight>
+                      <bold>true</bold>
+                     </font>
+                    </property>
+                    <property name="text">
+                     <string>Imaginary
+Dielectric
+Permittivity</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLCDNumber" name="science_imaginary_dielectric_lcd_number">
+                    <property name="minimumSize">
+                     <size>
+                      <width>0</width>
+                      <height>35</height>
+                     </size>
+                    </property>
+                   </widget>
+                  </item>
+                 </layout>
+                </item>
+               </layout>
+              </item>
+             </layout>
+            </item>
+           </layout>
+          </item>
          </layout>
         </widget>
         <widget class="QWidget" name="tab">

software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui

@@ -1551,7 +1551,7 @@ Position</string>
                     </font>
                    </property>
                    <property name="text">
-                    <string>Mode Readouts</string>
+                    <string>Mode Readouts / Setpoint</string>
                    </property>
                   </widget>
                  </item>
@@ -1801,6 +1801,34 @@ Position</string>
                      </property>
                     </spacer>
                    </item>
+                   <item>
+                    <layout class="QVBoxLayout" name="verticalLayout_68">
+                     <item>
+                      <widget class="QLabel" name="label_101">
+                       <property name="font">
+                        <font>
+                         <pointsize>10</pointsize>
+                         <weight>75</weight>
+                         <bold>true</bold>
+                        </font>
+                       </property>
+                       <property name="text">
+                        <string>Reported Setpoint</string>
+                       </property>
+                      </widget>
+                     </item>
+                     <item>
+                      <widget class="QLCDNumber" name="gripper_reported_setpoint_lcd_number">
+                       <property name="minimumSize">
+                        <size>
+                         <width>0</width>
+                         <height>50</height>
+                        </size>
+                       </property>
+                      </widget>
+                     </item>
+                    </layout>
+                   </item>
                   </layout>
                  </item>
                 </layout>
@@ -2201,8 +2229,23 @@ Position</string>
                      </item>
                     </layout>
                    </item>
+                  </layout>
+                 </item>
                  <item>
                   <layout class="QVBoxLayout" name="verticalLayout_15">
+                   <item>
+                    <spacer name="verticalSpacer_3">
+                     <property name="orientation">
+                      <enum>Qt::Vertical</enum>
+                     </property>
+                     <property name="sizeHint" stdset="0">
+                      <size>
+                       <width>20</width>
+                       <height>40</height>
+                      </size>
+                     </property>
+                    </spacer>
+                   </item>
                    <item>
                     <widget class="QLabel" name="label_3">
                      <property name="font">
@@ -2221,46 +2264,20 @@ Position</string>
                     </widget>
                    </item>
                    <item>
-                      <widget class="QProgressBar" name="speed_limit_progress_bar">
+                    <widget class="QSlider" name="rover_speed_limit_slider">
-                       <property name="sizePolicy">
-                        <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
-                         <horstretch>0</horstretch>
-                         <verstretch>0</verstretch>
-                        </sizepolicy>
-                       </property>
-                       <property name="maximumSize">
-                        <size>
-                         <width>133</width>
-                         <height>16777215</height>
-                        </size>
-                       </property>
                      <property name="maximum">
                       <number>100</number>
                      </property>
                      <property name="value">
                       <number>50</number>
                      </property>
-                       <property name="alignment">
-                        <set>Qt::AlignCenter</set>
-                       </property>
-                       <property name="textVisible">
-                        <bool>true</bool>
-                       </property>
                      <property name="orientation">
                       <enum>Qt::Horizontal</enum>
                      </property>
-                       <property name="invertedAppearance">
-                        <bool>false</bool>
-                       </property>
-                       <property name="textDirection">
-                        <enum>QProgressBar::TopToBottom</enum>
-                       </property>
                     </widget>
                    </item>
                   </layout>
                  </item>
-                  </layout>
-                 </item>
                  <item>
                   <spacer name="verticalSpacer_2">
                    <property name="orientation">

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/rover/ttyEffectors"
-DEFAULT_PORT = "/dev/ttyUSB1"
+# 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,20 +309,35 @@ class EffectorsControl(object):
     def process_gripper_control_message(self):
         if self.new_gripper_control_message:
 
+            self.gripper_registers[GRIPPER_MODBUS_REGISTERS["MODE"]] = self.gripper_control_message.gripper_mode
+
+            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
+
+            else:
                 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 -1 < gripper_absolute < UINT16_MAX:
-                self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(gripper_absolute, 0), GRIPPER_UNIVERSAL_POSITION_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), GRIPPER_UNIVERSAL_POSITION_MAX)
+                    self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(new_position, 0), UINT16_MAX)
 
                 self.gripper_node.write_registers(0, self.gripper_registers)
 
@@ -332,6 +347,7 @@ class EffectorsControl(object):
 
     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"]]

software/ros_packages/rover_main/launch/rover/science.launch

@@ -1,3 +1,5 @@
 <launch>
-    <node name="rover_science" pkg="rover_science" type="rover_science.py" respawn="true" output="screen"/>
+    <group ns="rover_science">
+        <node name="rover_science" pkg="rover_science" type="rover_science_node.py" respawn="true" output="screen"/>
+    </group>
 </launch>

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}
                 ]
             </rosparam>
         </node>

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()

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

software/ros_packages/rover_science/msg/RDFStatusMessage.msg

@@ -1,2 +0,0 @@
-uint16 raw_value
-double time

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
+

software/ros_packages/rover_science/package.xml

@@ -57,6 +57,7 @@
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
+  <exec_depend>message_generation</exec_depend>
   <exec_depend>std_msgs</exec_depend>
 
 

software/ros_packages/rover_science/src/rover_science.py

@@ -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()

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()

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