From 4aa9f7a5a8ff5f26161e7d3393b55f679157b044 Mon Sep 17 00:00:00 2001 From: marco-tranzatto Date: Mon, 18 Dec 2017 08:53:23 +0100 Subject: [PATCH 1/4] fix launch file geodetic survey --- piksi_multi_rtk_ros/launch/geodetic_survey.launch | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/piksi_multi_rtk_ros/launch/geodetic_survey.launch b/piksi_multi_rtk_ros/launch/geodetic_survey.launch index 43c37b89..4d9822e1 100644 --- a/piksi_multi_rtk_ros/launch/geodetic_survey.launch +++ b/piksi_multi_rtk_ros/launch/geodetic_survey.launch @@ -6,13 +6,10 @@ - - - - + - + From 6e301197fe33c8ea6dd059ac8b4b3b18d2cb51e3 Mon Sep 17 00:00:00 2001 From: marco-tranzatto Date: Mon, 18 Dec 2017 09:06:39 +0100 Subject: [PATCH 2/4] use message strings to set fix_mode. --- piksi_multi_rtk_ros/src/piksi_multi.py | 13 +++++++------ piksi_rtk_msgs/msg/ReceiverState_V2_2_15.msg | 7 +++++++ 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/piksi_multi_rtk_ros/src/piksi_multi.py b/piksi_multi_rtk_ros/src/piksi_multi.py index fa419dc6..bd98bdcc 100755 --- 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 index 5f72bdd8..724ba06e 100644 --- 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 From 3740e0e43cce302b2c963bd6f16d1ab78bf02fea Mon Sep 17 00:00:00 2001 From: marco-tranzatto Date: Mon, 18 Dec 2017 09:50:00 +0100 Subject: [PATCH 3/4] GUI uses similar colors style as Piksi consol for fix_mode. --- .../src/rqt_gps_rtk_plugin/GpsRtkPlugin.cpp | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) 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 index cf4ded8a..b2c15cef 100644 --- 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 &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 From 80baf05b2029bd631ee8c1c14f270110b8d4e7cc Mon Sep 17 00:00:00 2001 From: marco-tranzatto Date: Tue, 19 Dec 2017 17:49:38 +0100 Subject: [PATCH 4/4] remove debug print. --- piksi_multi_rtk_ros/src/geodetic_survey.py | 1 - 1 file changed, 1 deletion(-) diff --git a/piksi_multi_rtk_ros/src/geodetic_survey.py b/piksi_multi_rtk_ros/src/geodetic_survey.py index b2e21eed..f7dd6041 100755 --- 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.")