Merge pull request #82 from thomasemter/dev/next2

Fix spelling

CHANGELOG.rst

@@ -11,7 +11,7 @@ Changelog for package septentrio_gnss_driver
    * OSNMA
    * Latency compensation for ROS timestamps
    * Output of SBf block VelCovCartesian
-   * Support for UDP
+   * Support for UDP and TCP via IP server
    * New VSM handling allows for unknown variances (INS firmware >= 1.4.1)
    * Add heading angle to GPSFix msg (by diverting dip field, cf. readme)
 * Improvements

README.md

@@ -92,10 +92,14 @@ Conversions from LLA to UTM are incorporated through [GeographicLib](https://geo
     baudrate: 921600
     hw_flow_control: "off"
 
+  tcp:
+    ip_server: ""
+    port: 0
+
   udp:
-  ip_server: ""
-  port: 0
-  unicast_ip: ""
+    ip_server: ""
+    port: 0
+    unicast_ip: ""
 
   configure_rx: true
   
@@ -342,7 +346,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
   <details>
   <summary>Connectivity Specs</summary>
 
-  + `device`: location of device connection. This interface will be used for setup communication and VSM data for INS. SBF blocks and NMEA sentences are recevied either via this interface or UDP. The former will be utilized if section `udp` is not configured.
+  + `device`: location of main device connection. This interface will be used for setup communication and VSM data for INS. Incoming data streams of SBF blocks and NMEA sentences are recevied either via this interface or a static IP server for TCP and/or UDP. The former will be utilized if section `stream_device.tcp` and `stream_device.udp` are not configured.
     + `serial:xxx` format for serial connections,where xxx is the device node, e.g. `serial:/dev/ttyS0`. If using serial over USB, it is recommended to specify the port by ID as the Rx may get a different ttyXXX on reconnection, e.g. `serial:/dev/serial/by-id/usb-Septentrio_Septentrio_USB_Device_xyz`.
     + `file_name:path/to/file.sbf` format for publishing from an SBF log
     + `file_name:path/to/file.pcap` format for publishing from PCAP capture.
@@ -357,10 +361,14 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
     + `hw_flow_control`: specifies whether the serial (the Rx's COM ports, not USB1 or USB2) connection to the Rx should have UART hardware flow control enabled or not
       + `off` to disable UART hardware flow control, `RTS|CTS` to enable it
     + default: `921600`, `USB1`, `off`
-  + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences. If left unconfigured, intreface specified by `device` will be utilized.
-    + `ip_server`: IP server of Rx to be used, e.g. “IPS1”.
-    + `port`: UDP destination port.
-    + `unicast_ip`: Set to computer's IP to use unicast (optinal). If not set multicast will be used.
+  + `stream_device`: If left unconfigured, by default `device` is utilized for the data streams. Within `stream_device` static IP servers may be defined instead. In config mode (`configure_rx` set to `true`), TCP will be prioritized over UDP. If Rx is pre-configured, both may be set simultaneously.
+    + `tcp`: specifications for static TCP server of SBF blocks and NMEA sentences.
+      + `ip_server`: IP server of Rx to be used, e.g. “IPS1”.
+      + `port`: UDP destination port.
+    + `udp`: specifications for low latency UDP reception of SBF blocks and NMEA sentences.
+      + `ip_server`: IP server of Rx to be used, e.g. “IPS1”.
+      + `port`: UDP destination port.
+      + `unicast_ip`: Set to computer's IP to use unicast (optional). If not set multicast will be used.
   + `login`: credentials for user authentication to perform actions not allowed to anonymous users. Leave empty for anonymous access.
     + `user`: user name
     + `password`: password
@@ -381,7 +389,7 @@ The following is a list of ROSaic parameters found in the `config/rover.yaml` fi
   <details>
   <summary>Receiver Configuration</summary>
 
-    + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section, details can be found in section [ROS Topic Publications](#ros-topic-publications). Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*).
+    + configure_rx: Wether to configure the Rx according to the config file. If set to `false`, the Rx has to be configured via the web interface and the settings must be saved. On the driver side communication has to set accordingly to serial, TCP or UDP (TCP and UDP may even be used simultaneously in this case). For TCP communication it is recommended to use a static TCP server (`stream_device.tcp.ip_server` and `stream_device.tcp.port`), since dynamic connections (`device` is tcp) are not guaranteed to have the same id on reconnection. It should also be ensured that obligatory SBF blocks are activated (as of now: ReceiverTime if `use_gnss_time` is set to `true`; `PVTGeodetic`or `PVTCartesian` if latency compensation for PVT related blocks shall be used). Further, if ROS messages compiled from multiple SBF blocks, it should be ensured that all necessary blocks are activated with matching periods, details can be found in section [ROS Topic Publications](#ros-topic-publications). The messages that shall be published still have to be set to `true` in the *NMEA/SBF Messages to be Published* section. Also, parameters concerning the connection and node setup are still relevant (sections: *Connectivity Specs*, *receiver type*, *Frame IDs*, *UTM Zone Locking*, *Time Systems*, *Logger*).
       + default: true
   </details>
 

config/gnss.yaml

@@ -8,10 +8,14 @@ serial:
   baudrate: 921600
   hw_flow_control: "off"
 
-udp:
-  ip_server: ""
-  port: 0
-  unicast_ip: ""
+stream_device:
+  tcp:
+    ip_server: ""
+    port: 0
+  udp:
+    ip_server: ""
+    port: 0
+    unicast_ip: ""
 
 configure_rx: true
 

config/ins.yaml

@@ -8,10 +8,14 @@ serial:
   baudrate: 921600
   hw_flow_control: "off"
 
-udp:
-  ip_server: ""
-  port: 0
-  unicast_ip: ""
+stream_device:
+  tcp:
+    ip_server: ""
+    port: 0
+  udp:
+    ip_server: ""
+    port: 0
+    unicast_ip: ""
 
 configure_rx: true
 

config/rover.yaml

@@ -8,10 +8,14 @@ serial:
   baudrate: 921600
   hw_flow_control: "off"
 
-udp:
-  ip_server: ""
-  port: 0
-  unicast_ip: ""
+stream_device:
+  tcp:
+    ip_server: ""
+    port: 0
+  udp:
+    ip_server: ""
+    port: 0
+    unicast_ip: ""
 
 configure_rx: true
 

config/rover_node.yaml

@@ -8,11 +8,15 @@ septentrio_gnss_driver:
     serial:
       baudrate: 921600
       hw_flow_control: "off"
-
-    udp:
-      ip_server: ""
-      port: 0
-      unicast_ip: ""
+
+    stream_device:
+      tcp:
+        ip_server: ""
+        port: 0        
+      udp:
+        ip_server: ""
+        port: 0
+        unicast_ip: ""
 
     configure_rx: true    
 

include/septentrio_gnss_driver/communication/async_manager.hpp

@@ -119,6 +119,8 @@ namespace io {
 
         [[nodiscard]] bool connect();
 
+        void setPort(const std::string& port);
+
         void send(const std::string& cmd);
 
     private:
@@ -189,6 +191,12 @@ namespace io {
         return true;
     }
 
+    template <typename IoType>
+    void AsyncManager<IoType>::setPort(const std::string& port)
+    {
+        ioInterface_.setPort(port);
+    }
+
     template <typename IoType>
     void AsyncManager<IoType>::send(const std::string& cmd)
     {
@@ -251,6 +259,13 @@ namespace io {
                         std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                     receive();
                 }
+            } else if (running_ && std::is_same<TcpIo, IoType>::value)
+            {
+                // Send to check if TCP connection still alive
+                std::string empty = " ";
+                boost::asio::async_write(
+                    *(ioInterface_.stream_), boost::asio::buffer(empty.data(), 1),
+                    [](boost::system::error_code ec, std::size_t /*length*/) {});
             }
         }
     }

include/septentrio_gnss_driver/communication/communication_core.hpp

@@ -166,6 +166,7 @@ namespace io {
         //! This declaration is deliberately stream-independent (Serial or TCP).
         std::unique_ptr<AsyncManagerBase> manager_;
 
+        std::unique_ptr<AsyncManager<TcpIo>> tcpClient_;
         std::unique_ptr<UdpClient> udpClient_;
 
         bool nmeaActivated_ = false;

include/septentrio_gnss_driver/communication/io.hpp

@@ -242,12 +242,15 @@ namespace io {
             node_(node),
             ioService_(ioService)
         {
+            port_ = node_->settings()->device_tcp_port;
         }
 
         ~TcpIo() { stream_->close(); }
 
         void close() { stream_->close(); }
 
+        void setPort(const std::string& port) { port_ = port; }
+
         [[nodiscard]] bool connect()
         {
             boost::asio::ip::tcp::resolver::iterator endpointIterator;
@@ -256,22 +259,21 @@ namespace io {
             {
                 boost::asio::ip::tcp::resolver resolver(*ioService_);
                 boost::asio::ip::tcp::resolver::query query(
-                    node_->settings()->tcp_ip, node_->settings()->tcp_port);
+                    node_->settings()->device_tcp_ip, port_);
                 endpointIterator = resolver.resolve(query);
             } catch (std::runtime_error& e)
             {
                 node_->log(log_level::ERROR,
-                           "Could not resolve " + node_->settings()->tcp_ip +
-                               " on port " + node_->settings()->tcp_port + ": " +
-                               e.what());
+                           "Could not resolve " + node_->settings()->device_tcp_ip +
+                               " on port " + port_ + ": " + e.what());
                 return false;
             }
 
             stream_.reset(new boost::asio::ip::tcp::socket(*ioService_));
 
             node_->log(log_level::INFO, "Connecting to tcp://" +
-                                            node_->settings()->tcp_ip + ":" +
-                                            node_->settings()->tcp_port + "...");
+                                            node_->settings()->device_tcp_ip + ":" +
+                                            port_ + "...");
 
             try
             {
@@ -297,6 +299,8 @@ namespace io {
         ROSaicNodeBase* node_;
         std::shared_ptr<boost::asio::io_service> ioService_;
 
+        std::string port_;
+
     public:
         std::unique_ptr<boost::asio::ip::tcp::socket> stream_;
     };

include/septentrio_gnss_driver/communication/settings.hpp

@@ -129,15 +129,19 @@ struct Settings
     //! Device type
     device_type::DeviceType device_type;
     //! TCP IP
-    std::string tcp_ip;
+    std::string device_tcp_ip;
     //! TCP port
-    std::string tcp_port;
+    std::string device_tcp_port;
     //! UDP port
     uint32_t udp_port;
     //! UDP unicast destination ip
     std::string udp_unicast_ip;
     //! UDP IP server id
     std::string udp_ip_server;
+    //! TCP port
+    uint32_t tcp_port;
+    //! TCP IP server id
+    std::string tcp_ip_server;
     //! Filename
     std::string file_name;
     //! Username for login

msg/MeasEpochChannelType2.msg

@@ -6,6 +6,6 @@ uint8  cn0                # 0.25 dB-Hz
 uint8  offsets_msb        # 65.536 m or 65.536 Hz
 int8   carrier_msb        # 65.536 cycles
 uint8  obs_info
-uint16 code_offset_lsb
-uint16 carrier_lsb
-uint16 doppler_offset_lsb
+uint16 code_offset_lsb    # 0.001 m
+uint16 carrier_lsb        # 0.001 cycles
+uint16 doppler_offset_lsb # 0.0001 Hz

msg/VelCovGeodetic.msg

@@ -7,13 +7,13 @@ BlockHeader  block_header
 
 uint8        mode
 uint8        error        
-float32      cov_vnvn
-float32      cov_veve
-float32      cov_vuvu
-float32      cov_dtdt
-float32      cov_vnve
-float32      cov_vnvu
-float32      cov_vndt
-float32      cov_vevu
-float32      cov_vedt
-float32      cov_vudt
+float32      cov_vnvn # m^2/s^2
+float32      cov_veve # m^2/s^2
+float32      cov_vuvu # m^2/s^2
+float32      cov_dtdt # m^2/s^2
+float32      cov_vnve # m^2/s^2
+float32      cov_vnvu # m^2/s^2
+float32      cov_vndt # m^2/s^2
+float32      cov_vevu # m^2/s^2
+float32      cov_vedt # m^2/s^2
+float32      cov_vudt # m^2/s^2

src/septentrio_gnss_driver/communication/communication_core.cpp

@@ -203,13 +203,21 @@ namespace io {
 
     [[nodiscard]] bool CommunicationCore::initializeIo()
     {
-        bool udp = false;
+        bool client = false;
         node_->log(log_level::DEBUG, "Called initializeIo() method");
+        if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty()))
+        {
+            tcpClient_.reset(new AsyncManager<TcpIo>(node_, &telegramQueue_));
+            tcpClient_->setPort(std::to_string(settings_->tcp_port));
+            if (!settings_->configure_rx)
+                tcpClient_->connect();
+            client = true;
+        }
         if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty()))
         {
             udpClient_.reset(
                 new UdpClient(node_, settings_->udp_port, &telegramQueue_));
-            udp = true;
+            client = true;
         }
 
         switch (settings_->device_type)
@@ -236,7 +244,7 @@ namespace io {
         }
         default:
         {
-            if (!udp || settings_->configure_rx ||
+            if (!client || settings_->configure_rx ||
                 (settings_->ins_vsm_ros_source == "odometry") ||
                 (settings_->ins_vsm_ros_source == "twist"))
             {
@@ -277,7 +285,13 @@ namespace io {
         node_->log(log_level::INFO,
                    "The connection descriptor is " + mainConnectionPort_);
         streamPort_ = mainConnectionPort_;
-        if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty()))
+        if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty()))
+        {
+            streamPort_ = settings_->tcp_ip_server;
+            send("siss, " + streamPort_ + ", " +
+                 std::to_string(settings_->tcp_port) + ", TCP, " + "\x0D");
+            tcpClient_->connect();
+        } else if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty()))
         {
             streamPort_ = settings_->udp_ip_server;
             std::string destination;