Merge pull request #45 from thomasemter/dev/refactor

Dev/refactor
septentrio-gnss · Jan 2, 2022 · 239d8cb · 239d8cb
2 parents 282ff9f + 466390a
commit 239d8cb
Show file tree

Hide file tree

Showing 10 changed files with 151 additions and 72 deletions.
diff --git a/README.md b/README.md
@@ -259,6 +259,8 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
     + If `gnss`, then ROS can only output data related to GNSS receivers.
     + If `ins`, then ROS can only output data related to INS receivers.
 	+ default: `gnss`
+  + `multi_antenna`: Whether or not the Rx has multiple antennas.
+      + default: `false`
   </details>
 
   <details>
@@ -275,9 +277,9 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
     + default: `vsm`
   + `aux1_frame_id`: name of the ROS tf frame for the aux1 antenna.
     + default: `aux1`
-   + `vehicle_frame_id`: name of the ROS tf frame for the aux1 antenna.
+  + `vehicle_frame_id`: name of the ROS tf frame for the aux1 antenna.
     + default: `base_link`
-  + `get_spatial_config_from_tf`: wether to get the spatial config via tf with the above mentioned frame ids. This will override spatial settings of the config file. Keep in mind that tf has a tree structure. Thus, if the POI is the vehicle frame, transfrom from IMU to POI is given as inverse of vehicle frame to IMU. 
+  + `get_spatial_config_from_tf`: wether to get the spatial config via tf with the above mentioned frame ids. This will override spatial settings of the config file. For receiver type `ins` with `multi_antenna` set to `true` all frames have to be provided, with `multi_antenna` set to `false`, `aux1_frame_id` is not necessary. For type `gnss` with dual-antenna setup only `frame_id`, `aux1_frame_id`, and `vehicle_frame_id` are needed. Do not use for single-antenna `gnss`. Keep in mind that tf has a tree structure. Thus, if the POI is the vehicle frame, transfrom from IMU to POI is given as inverse of vehicle frame to IMU. 
     + default: `false`
   + `lock_utm_zone`: wether the UTM zone of the first localization is locked
     + default: `true`
@@ -390,9 +392,9 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
     + `ins_std_dev_mask`: Maximum accepted error
       + `att_std_dev`: Configures an output limit on standard deviation of the attitude angles (max error accepted: 5 degrees)
       + `pos_std_dev`: Configures an output limit on standard deviation of the position (max error accepted: 100 meters)
-      + default: `2` degrees, `100` meters
+      + default: `2` degrees, `100` meters    
     + `ins_use_poi`: Whether or not to use the POI defined in `ins_spatial_config/poi_to_imu`
-      + If true, the point at which the INS navigation solution (e.g. in `insnavgeod` ROS topic) is calculated will be the POI as defined above, otherwise it'll be the main GNSS antenna.
+      + If true, the point at which the INS navigation solution (e.g. in `insnavgeod` ROS topic) is calculated will be the POI as defined above (`poi_frame_id`), otherwise it'll be the main GNSS antenna (`frame_id`).
       + default: `false`
   </details>
 

diff --git a/config/rover.yaml b/config/rover.yaml
@@ -19,6 +19,8 @@ vsm_frame_id: vsm
 
 aux1_frame_id: aux1
 
+vehicle_frame_id: base_link
+
 get_spatial_config_from_tf: false
 
 lock_utm_zone: true
@@ -27,6 +29,8 @@ use_ros_axis_orientation: true
 
 receiver_type: gnss
 
+multi_antenna: false
+
 datum: ETRS89
 
 poi_to_arp:

diff --git a/include/septentrio_gnss_driver/abstraction/typedefs.hpp b/include/septentrio_gnss_driver/abstraction/typedefs.hpp
@@ -83,6 +83,7 @@ typedef diagnostic_msgs::DiagnosticStatusPtr        DiagnosticStatusMsgPtr;
 typedef geometry_msgs::Quaternion                   QuaternionMsg;
 typedef geometry_msgs::PoseWithCovarianceStamped    PoseWithCovarianceStampedMsg;
 typedef geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedMsgPtr;
+typedef geometry_msgs::TransformStamped             TransformStampedMsg;
 typedef gps_common::GPSFix                          GPSFixMsg;
 typedef gps_common::GPSFixPtr                       GPSFixMsgPtr;
 typedef gps_common::GPSStatus                       GPSStatusMsg;

diff --git a/include/septentrio_gnss_driver/communication/rx_message.hpp b/include/septentrio_gnss_driver/communication/rx_message.hpp
@@ -211,6 +211,8 @@ struct Settings
     double heading_offset;
     //! Attitude offset determination in latitudinal direction
     double pitch_offset;
+    //! INS multiantenna
+    bool multi_antenna;
     //! INS solution reference point
     bool ins_use_poi;
     //! For heading computation when unit is powered-cycled

diff --git a/include/septentrio_gnss_driver/node/rosaic_node.hpp b/include/septentrio_gnss_driver/node/rosaic_node.hpp
@@ -105,15 +105,15 @@ namespace rosaic_node {
          * @param[in] sourceFrame source frame id
          * @param[out] T_s_t transfrom from source to target
          */
-        void getTransform(const std::string& targetFrame, const std::string& sourceFrame, geometry_msgs::TransformStamped& T_s_t);
+        void getTransform(const std::string& targetFrame, const std::string& sourceFrame, TransformStampedMsg& T_s_t);
         /**
          * @brief Gets Euler angles from quaternion message
          * @param[in] qm quaternion message
          * @param[out] roll roll angle
          * @param[out] pitch pitch angle
          * @param[out] yaw yaw angle
          */
-        void getRPY(const geometry_msgs::Quaternion& qm, double& roll, double& pitch, double& yaw);
+        void getRPY(const QuaternionMsg& qm, double& roll, double& pitch, double& yaw);
 
         //! Settings
         Settings settings_;

diff --git a/include/septentrio_gnss_driver/parsers/string_utilities.h b/include/septentrio_gnss_driver/parsers/string_utilities.h
@@ -143,6 +143,13 @@ namespace string_utilities {
      * @return The trimmed string
      */
     std::string trimString(std::string str);
+
+    /**
+     * @brief Trims decimal places to two
+     * @param[in] num The double who shall be trimmed
+     * @return The string
+     */
+    std::string trimDecimalPlaces(double num);
 } // namespace string_utilities
 
 #ifdef __cplusplus

diff --git a/src/septentrio_gnss_driver/communication/communication_core.cpp b/src/septentrio_gnss_driver/communication/communication_core.cpp
@@ -638,13 +638,23 @@ void io_comm_rx::Comm_IO::configureRx()
         }
     }
 
+    // Setting multi antenna
+    if (settings_->multi_antenna)
+    {
+        send("sga, MultiAntenna \x0D");
+    }
+    else
+    {
+        send("sga, none \x0D");
+    }
+
     // Setting the Attitude Determination
     {
         if (settings_->heading_offset >= HEADING_MIN && settings_->heading_offset<= HEADING_MAX && settings_->pitch_offset >= PITCH_MIN && settings_->pitch_offset <= PITCH_MAX)
         {
             std::stringstream ss;
-            ss << "sto, " << string_utilities::trimString(std::to_string(settings_->heading_offset))
-            << ", " << string_utilities::trimString(std::to_string(settings_->pitch_offset)) << " \x0D";
+            ss << "sto, " << string_utilities::trimDecimalPlaces(settings_->heading_offset)
+            << ", " << string_utilities::trimDecimalPlaces(settings_->pitch_offset) << " \x0D";
             send(ss.str());
         }
         else

diff --git a/src/septentrio_gnss_driver/communication/rx_message.cpp b/src/septentrio_gnss_driver/communication/rx_message.cpp
@@ -1711,7 +1711,7 @@ NavSatFixMsgPtr io_comm_rx::RxMessage::NavSatFixCallback()
 		default:
 		{
 			node_->log(LogLevel::DEBUG, "INSNavGeod's Mode field contains an invalid type of PVT solution.");
-            break;
+			break;
 		}
 		}
 		bool gps_in_pvt = false;

diff --git a/src/septentrio_gnss_driver/node/rosaic_node.cpp b/src/septentrio_gnss_driver/node/rosaic_node.cpp
@@ -126,6 +126,30 @@ bool rosaic_node::ROSaicNode::getROSParams()
         return false;
     }
 
+    // Publishing parameters, the others remained global since they need to be
+    // accessed in the callbackhandlers.hpp file
+    param("publish/gpgga", settings_.publish_gpgga, true);
+    param("publish/gprmc", settings_.publish_gprmc, true);
+    param("publish/gpgsa", settings_.publish_gpgsa, true);
+    param("publish/gpgsv", settings_.publish_gpgsv, true);
+    param("publish/pvtcartesian", settings_.publish_pvtcartesian, true);
+    param("publish/pvtgeodetic", settings_.publish_pvtgeodetic, true);
+    param("publish/poscovcartesian", settings_.publish_poscovcartesian, true);
+    param("publish/poscovgeodetic", settings_.publish_poscovgeodetic, true);
+	param("publish/velcovgeodetic", settings_.publish_velcovgeodetic, true);
+    param("publish/atteuler", settings_.publish_atteuler, true);
+    param("publish/attcoveuler", settings_.publish_attcoveuler, true);
+    param("publish/insnavcart", settings_.publish_insnavcart, true);
+    param("publish/insnavgeod", settings_.publish_insnavgeod, true);
+    param("publish/imusetup", settings_.publish_imusetup, true);
+    param("publish/velsensorsetup", settings_.publish_velsensorsetup, true);
+    param("publish/exteventinsnavgeod", settings_.publish_exteventinsnavgeod, true);
+    param("publish/exteventinsnavcart", settings_.publish_exteventinsnavcart, true);
+    param("publish/extsensormeas", settings_.publish_extsensormeas, true);
+    param("publish/imu", settings_.publish_imu, true);
+    param("publish/localization", settings_.publish_localization, true);
+    param("publish/tf", settings_.publish_tf, true);
+
     // Datum and marker-to-ARP offset
     param("datum", settings_.datum, std::string("ETRS89"));
     param("ant_type", settings_.ant_type, std::string("Unknown"));
@@ -153,48 +177,73 @@ bool rosaic_node::ROSaicNode::getROSParams()
     param("poi_to_arp/delta_u", settings_.delta_u, 0.0f);
 
     param("use_ros_axis_orientation", settings_.use_ros_axis_orientation, true);
+
+    // multi_antenna param
+    param("multi_antenna", settings_.multi_antenna, false);
 
 	// INS Spatial Configuration
     bool getConfigFromTf;
     param("get_spatial_config_from_tf", getConfigFromTf, false);
     if (getConfigFromTf)
     {
-        geometry_msgs::TransformStamped T_imu_base;
-        getTransform(settings_.imu_frame_id, settings_.vehicle_frame_id, T_imu_base);
-        geometry_msgs::TransformStamped T_poi_imu;
-        getTransform(settings_.imu_frame_id, settings_.poi_frame_id, T_poi_imu);
-        geometry_msgs::TransformStamped T_vsm_imu;
-        getTransform(settings_.imu_frame_id, settings_.vsm_frame_id, T_vsm_imu);
-        geometry_msgs::TransformStamped T_ant_imu;
-        getTransform(settings_.imu_frame_id, settings_.frame_id, T_ant_imu);
-        geometry_msgs::TransformStamped T_aux1_imu;
-        getTransform(settings_.imu_frame_id, settings_.aux1_frame_id, T_aux1_imu);
-
-        // IMU orientation parameter
-        double roll, pitch, yaw;
-        getRPY(T_imu_base.transform.rotation, roll, pitch, yaw);
-        settings_.theta_x = parsing_utilities::rad2deg(roll);
-        settings_.theta_y = parsing_utilities::rad2deg(pitch);
-        settings_.theta_z = parsing_utilities::rad2deg(yaw);
-        // INS antenna lever arm offset parameter
-        settings_.ant_lever_x = T_ant_imu.transform.translation.x;
-        settings_.ant_lever_y = T_ant_imu.transform.translation.y;
-        settings_.ant_lever_z = T_ant_imu.transform.translation.z;
-        // INS POI ofset paramter
-        settings_.poi_x = T_poi_imu.transform.translation.x;
-        settings_.poi_y = T_poi_imu.transform.translation.y;
-        settings_.poi_z = T_poi_imu.transform.translation.z;
-        // INS velocity sensor lever arm offset parameter
-        settings_.vsm_x = T_vsm_imu.transform.translation.x;
-        settings_.vsm_y = T_vsm_imu.transform.translation.y;
-        settings_.vsm_z = T_vsm_imu.transform.translation.z;
-        // Antenna Attitude Determination parameter
-        double dy = T_aux1_imu.transform.translation.y - T_ant_imu.transform.translation.y;
-        double dx = T_aux1_imu.transform.translation.x - T_ant_imu.transform.translation.x;
-        settings_.heading_offset = parsing_utilities::rad2deg(std::atan2(dy, dx));
-        double dz = T_aux1_imu.transform.translation.z - T_ant_imu.transform.translation.z;
-        double dr = std::sqrt(parsing_utilities::square(dx) + parsing_utilities::square(dy));
-        settings_.pitch_offset = parsing_utilities::rad2deg(std::atan2(-dz, dr));
+        if (settings_.septentrio_receiver_type == "ins")
+        {
+            TransformStampedMsg T_imu_base;
+            getTransform(settings_.imu_frame_id, settings_.vehicle_frame_id, T_imu_base);
+            TransformStampedMsg T_poi_imu;
+            getTransform(settings_.imu_frame_id, settings_.poi_frame_id, T_poi_imu);
+            TransformStampedMsg T_vsm_imu;
+            getTransform(settings_.imu_frame_id, settings_.vsm_frame_id, T_vsm_imu);
+            TransformStampedMsg T_ant_imu;
+            getTransform(settings_.imu_frame_id, settings_.frame_id, T_ant_imu);           
+
+            // IMU orientation parameter
+            double roll, pitch, yaw;
+            getRPY(T_imu_base.transform.rotation, roll, pitch, yaw);
+            settings_.theta_x = parsing_utilities::rad2deg(roll);
+            settings_.theta_y = parsing_utilities::rad2deg(pitch);
+            settings_.theta_z = parsing_utilities::rad2deg(yaw);
+            // INS antenna lever arm offset parameter
+            settings_.ant_lever_x = T_ant_imu.transform.translation.x;
+            settings_.ant_lever_y = T_ant_imu.transform.translation.y;
+            settings_.ant_lever_z = T_ant_imu.transform.translation.z;
+            // INS POI ofset paramter
+            settings_.poi_x = T_poi_imu.transform.translation.x;
+            settings_.poi_y = T_poi_imu.transform.translation.y;
+            settings_.poi_z = T_poi_imu.transform.translation.z;
+            // INS velocity sensor lever arm offset parameter
+            settings_.vsm_x = T_vsm_imu.transform.translation.x;
+            settings_.vsm_y = T_vsm_imu.transform.translation.y;
+            settings_.vsm_z = T_vsm_imu.transform.translation.z;
+
+            if (settings_.multi_antenna)
+            {
+                TransformStampedMsg T_aux1_imu;
+                getTransform(settings_.imu_frame_id, settings_.aux1_frame_id, T_aux1_imu);
+                // Antenna Attitude Determination parameter
+                double dy = T_aux1_imu.transform.translation.y - T_ant_imu.transform.translation.y;
+                double dx = T_aux1_imu.transform.translation.x - T_ant_imu.transform.translation.x;
+                settings_.heading_offset = parsing_utilities::rad2deg(std::atan2(dy, dx));
+                double dz = T_aux1_imu.transform.translation.z - T_ant_imu.transform.translation.z;
+                double dr = std::sqrt(parsing_utilities::square(dx) + parsing_utilities::square(dy));
+                settings_.pitch_offset = parsing_utilities::rad2deg(std::atan2(-dz, dr));
+            }
+        }
+        if ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna)
+        {
+            TransformStampedMsg T_ant_base;
+            getTransform(settings_.vehicle_frame_id, settings_.frame_id, T_ant_base);
+            TransformStampedMsg T_aux1_base;
+            getTransform(settings_.vehicle_frame_id, settings_.aux1_frame_id, T_aux1_base);
+
+            // Antenna Attitude Determination parameter
+            double dy = T_aux1_base.transform.translation.y - T_ant_base.transform.translation.y;
+            double dx = T_aux1_base.transform.translation.x - T_ant_base.transform.translation.x;
+            settings_.heading_offset = parsing_utilities::rad2deg(std::atan2(dy, dx));
+            double dz = T_aux1_base.transform.translation.z - T_ant_base.transform.translation.z;
+            double dr = std::sqrt(parsing_utilities::square(dx) + parsing_utilities::square(dy));
+            settings_.pitch_offset = parsing_utilities::rad2deg(std::atan2(-dz, dr));
+        }
     }
     else
     {
@@ -232,6 +281,20 @@ bool rosaic_node::ROSaicNode::getROSParams()
         settings_.pitch_offset   *= -1.0;
     }
 
+    if (settings_.heading_offset != 0.0)
+    {
+        if (settings_.publish_atteuler)
+        {
+            this->log(LogLevel::WARN , "Pitch angle output by topic /atteuler is a tilt angle rotated by " + 
+                                    std::to_string(settings_.heading_offset) + ".");
+        }
+        if (settings_.publish_pose && (settings_.septentrio_receiver_type == "gnss"))
+        {
+            this->log(LogLevel::WARN , "Pitch angle output by topic /pose is a tilt angle rotated by " + 
+                                        std::to_string(settings_.heading_offset) + ".");
+        }
+    }
+
     this->log(LogLevel::DEBUG , "IMU roll offset: "+ std::to_string(settings_.theta_x));
     this->log(LogLevel::DEBUG , "IMU pitch offset: "+ std::to_string(settings_.theta_y));
     this->log(LogLevel::DEBUG , "IMU yaw offset: "+ std::to_string(settings_.theta_z));
@@ -268,31 +331,7 @@ bool rosaic_node::ROSaicNode::getROSParams()
     getUint32Param("ntrip_settings/rx_input_corrections_tcp", settings_.rx_input_corrections_tcp,
               static_cast<uint32_t>(28785));
     param("ntrip_settings/rx_input_corrections_serial",
-                settings_.rx_input_corrections_serial, std::string("USB2"));
-
-    // Publishing parameters, the others remained global since they need to be
-    // accessed in the callbackhandlers.hpp file
-    param("publish/gpgga", settings_.publish_gpgga, true);
-    param("publish/gprmc", settings_.publish_gprmc, true);
-    param("publish/gpgsa", settings_.publish_gpgsa, true);
-    param("publish/gpgsv", settings_.publish_gpgsv, true);
-    param("publish/pvtcartesian", settings_.publish_pvtcartesian, true);
-    param("publish/pvtgeodetic", settings_.publish_pvtgeodetic, true);
-    param("publish/poscovcartesian", settings_.publish_poscovcartesian, true);
-    param("publish/poscovgeodetic", settings_.publish_poscovgeodetic, true);
-	param("publish/velcovgeodetic", settings_.publish_velcovgeodetic, true);
-    param("publish/atteuler", settings_.publish_atteuler, true);
-    param("publish/attcoveuler", settings_.publish_attcoveuler, true);
-    param("publish/insnavcart", settings_.publish_insnavcart, true);
-    param("publish/insnavgeod", settings_.publish_insnavgeod, true);
-    param("publish/imusetup", settings_.publish_imusetup, true);
-    param("publish/velsensorsetup", settings_.publish_velsensorsetup, true);
-    param("publish/exteventinsnavgeod", settings_.publish_exteventinsnavgeod, true);
-    param("publish/exteventinsnavcart", settings_.publish_exteventinsnavcart, true);
-    param("publish/extsensormeas", settings_.publish_extsensormeas, true);
-    param("publish/imu", settings_.publish_imu, true);
-    param("publish/localization", settings_.publish_localization, true);
-    param("publish/tf", settings_.publish_tf, true);
+                settings_.rx_input_corrections_serial, std::string("USB2"));    
 
     // Automatically activate needed sub messages
     if (settings_.septentrio_receiver_type == "gnss")
@@ -360,7 +399,7 @@ bool rosaic_node::ROSaicNode::getROSParams()
     return true;
 }
 
-void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, const std::string& sourceFrame, geometry_msgs::TransformStamped& T_s_t)
+void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, const std::string& sourceFrame, TransformStampedMsg& T_s_t)
 {
     bool found = false;
     while (!found)
@@ -379,7 +418,7 @@ void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame, const
     }
 }
 
-void rosaic_node::ROSaicNode::getRPY(const geometry_msgs::Quaternion& qm, double& roll, double& pitch, double& yaw)
+void rosaic_node::ROSaicNode::getRPY(const QuaternionMsg& qm, double& roll, double& pitch, double& yaw)
 {
     Eigen::Quaterniond q(qm.w, qm.x, qm.y, qm.z);
 	Eigen::Quaterniond::RotationMatrixType C = q.matrix();

diff --git a/src/septentrio_gnss_driver/parsers/string_utilities.cpp b/src/septentrio_gnss_driver/parsers/string_utilities.cpp
@@ -34,6 +34,9 @@
 #include <cerrno>
 #include <cstdlib>
 #include <limits>
+#include <sstream>
+#include <iomanip>
+#include <cmath>
 
 /**
  * @file string_utilities.cpp
@@ -193,4 +196,15 @@ namespace string_utilities {
         }
         return str;
     }
+
+    std::string trimDecimalPlaces(double num)
+    {
+        num = std::round(num * 1000);
+        num = num / 1000;
+        std::stringstream ss;
+        ss << std::fixed;
+        ss << std::setprecision(3);
+        ss << num;
+        return ss.str();
+    }
 } // namespace string_utilities