Merge pull request #4 from ethz-asl/fix/after_hoengg_test

Fixes after Hoengg test
ethz-asl · Dec 21, 2017 · 600d759 · 600d759
2 parents f8f4aa8 + 80baf05
commit 600d759
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 16 deletions.
diff --git a/piksi_multi_rtk_ros/launch/geodetic_survey.launch b/piksi_multi_rtk_ros/launch/geodetic_survey.launch
@@ -6,13 +6,10 @@
   <arg name="number_of_desired_fixes"     default="5000"/>
 
   <!-- Piksi. -->
-  <node pkg="piksi_multi_rtk_gps" type="piksi_multi.py" name="piksi" output="screen">
-    <!-- Load default settings -->
-    <rosparam file="$(find piksi_multi_rtk_gps)/cfg/piksi_multi_driver_settings.yaml"/>
-  </node>
+  <include file="$(find piksi_multi_rtk_ros)/launch/piksi_multi_base_station.launch"/>
 
   <!-- Geodetic Survey. -->
-  <node pkg="piksi_multi_rtk_gps" type="geodetic_survey.py" name="geodetic_survey" output="screen">
+  <node pkg="piksi_multi_rtk_ros" type="geodetic_survey.py" name="geodetic_survey" output="screen">
     <param name="number_of_desired_fixes" value="$(arg number_of_desired_fixes)"/>
   </node>
 

diff --git a/piksi_multi_rtk_ros/src/geodetic_survey.py b/piksi_multi_rtk_ros/src/geodetic_survey.py
@@ -102,7 +102,6 @@ def set_base_station_position(self, lat0, lon0, alt0):
                             "surveyed_position", "broadcast")
 
                         read_surveyed_broadcast_value = True if (read_surveyed_broadcast_value == "True") else False
-                        print read_surveyed_broadcast_value
                         if read_surveyed_broadcast_value:
                             rospy.loginfo(
                                 "Your Piksi Multi is already configured to broadcast its surveyed position and act as base station.")

diff --git a/piksi_multi_rtk_ros/src/piksi_multi.py b/piksi_multi_rtk_ros/src/piksi_multi.py
@@ -257,6 +257,7 @@ def init_receiver_state_msg(self):
         receiver_state_msg.cn0_sbas = []  # Unknown.
         receiver_state_msg.num_glonass_sat = 0  # Unknown.
         receiver_state_msg.cn0_glonass = []  # Unknown.
+        receiver_state_msg.fix_mode = ReceiverState_V2_2_15.STR_FIX_MODE_UNKNOWN
 
         return receiver_state_msg
 
@@ -531,17 +532,17 @@ def pos_llh_callback(self, msg_raw, **metadata):
         self.receiver_state_msg.rtk_mode_fix = True if (msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK) else False
 
         if msg.flags == PosLlhMulti.FIX_MODE_INVALID:
-            self.receiver_state_msg.fix_mode = "Invalid"
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_2_15.STR_FIX_MODE_INVALID
         elif msg.flags == PosLlhMulti.FIX_MODE_SPP:
-            self.receiver_state_msg.fix_mode = "SPP"
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_2_15.STR_FIX_MODE_SPP
         elif msg.flags == PosLlhMulti.FIX_MODE_DGNSS:
-            self.receiver_state_msg.fix_mode = "DGNSS"
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_2_15.STR_FIX_MODE_DGNSS
         elif msg.flags == PosLlhMulti.FIX_MODE_FLOAT_RTK:
-            self.receiver_state_msg.fix_mode = "Float_RTK"
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_2_15.STR_FIX_MODE_FLOAT_RTK
         elif msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK:
-            self.receiver_state_msg.fix_mode = "Fixed_RTK"
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_2_15.STR_FIX_MODE_FIXED_RTK
         else:
-            self.receiver_state_msg.fix_mode = "Unknown"
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_2_15.STR_FIX_MODE_UNKNOWN
 
         self.publish_receiver_state_msg()
 

diff --git a/piksi_rtk_msgs/msg/ReceiverState_V2_2_15.msg b/piksi_rtk_msgs/msg/ReceiverState_V2_2_15.msg
@@ -29,3 +29,10 @@ uint8 CODE_GLO_L1CA     = 3
 uint8 CODE_GLO_L2CA     = 4
 uint8 CODE_GPS_L1P      = 5
 uint8 CODE_GPS_L2P      = 6
+
+string STR_FIX_MODE_INVALID   = Invalid
+string STR_FIX_MODE_SPP       = SPP
+string STR_FIX_MODE_DGNSS     = DGNSS
+string STR_FIX_MODE_FLOAT_RTK = FLOAT_RTK
+string STR_FIX_MODE_FIXED_RTK = FIXED_RTK
+string STR_FIX_MODE_UNKNOWN   = Unknown
diff --git a/rqt_gps_rtk_plugin/src/rqt_gps_rtk_plugin/GpsRtkPlugin.cpp b/rqt_gps_rtk_plugin/src/rqt_gps_rtk_plugin/GpsRtkPlugin.cpp
@@ -119,10 +119,27 @@ void GpsRtkPlugin::vectorToString(const std::vector<uint8_t> &vec, QString *pStr
 
 void GpsRtkPlugin::piksiReceiverStateCb(const piksi_rtk_msgs::ReceiverState_V2_2_15& msg) {
   // Type of fix
-  QMetaObject::invokeMethod(ui_.label_fixType, "setText", Q_ARG(QString, msg.rtk_mode_fix ? "Fix" : "Float"));
-  QMetaObject::invokeMethod(ui_.label_fixType, "setStyleSheet", Q_ARG(QString, msg.rtk_mode_fix ?
-        "QLabel {background-color: lime; color: black;}"
-      : "QLabel {background-color: rgb(239, 41, 41); color: rgb(0, 0, 0);}"));
+  const QString fix_mode = QString::fromStdString(msg.fix_mode);
+  QString color_fix_mode("");
+
+  // Choose color for type of fix
+  if (msg.fix_mode == msg.STR_FIX_MODE_INVALID) {
+    color_fix_mode = "QLabel {background-color: rgb(239, 41, 41); color: rgb(92, 92, 92);}";
+  } else if (msg.fix_mode == msg.STR_FIX_MODE_SPP) {
+    color_fix_mode = "QLabel {background-color: rgb(255, 255, 255); color: rgb(2, 2, 255);}";
+  } else if (msg.fix_mode == msg.STR_FIX_MODE_DGNSS) {
+    color_fix_mode = "QLabel {background-color: rgb(255, 255, 255); color: rgb(5, 181, 255);}";
+  } else if (msg.fix_mode == msg.STR_FIX_MODE_FLOAT_RTK) {
+    color_fix_mode = "QLabel {background-color: rgb(255, 138, 138); color: rgb(191, 0, 191);}";
+  } else if (msg.fix_mode == msg.STR_FIX_MODE_FIXED_RTK) {
+    color_fix_mode = "QLabel {background-color: lime; color: rgb(255, 166, 2);}";
+  } else {
+    // Unknown
+    color_fix_mode = "QLabel {background-color: rgb(152, 152, 152); color: rgb(92, 92, 92);}";
+  }
+
+  QMetaObject::invokeMethod(ui_.label_fixType, "setText", Q_ARG(QString, fix_mode));
+  QMetaObject::invokeMethod(ui_.label_fixType, "setStyleSheet", Q_ARG(QString, color_fix_mode));
   // Number of satellites
   QMetaObject::invokeMethod(ui_.label_numSatellites, "setText", Q_ARG(QString, QString::number(msg.num_sat)));
   // GPS number of satellites