Ntrip msg type print and disable dropping len>255 (#13)

* Workaround print that helps to get a connection to RTK server

* Add entrypoint script to exit properly

* Add some debug prints and pub until max len

* Fix the message type inspection

---------

Co-authored-by: Manuel Segarra-Abad <manuel.segarra-abad@unikie.com>

Dockerfile

@@ -18,6 +18,7 @@ RUN apt-get update \
         nmea-msgs \
     && rm -rf /var/lib/apt/lists/*
 
-ENTRYPOINT ros-with-env ros2 launch ntrip_client ntrip_client_launch.py
+COPY entrypoint.sh /entrypoint.sh
+ENTRYPOINT [ "/entrypoint.sh" ]
 
 COPY --from=builder $INSTALL_DIR $INSTALL_DIR

entrypoint.sh

@@ -0,0 +1,42 @@
+#!/bin/bash -e
+
+_term() {
+    # FILL UP PROCESS SEARCH PATTERN HERE TO FIND PROPER PROCESS FOR SIGINT:
+    pattern="ntrip_client/ntrip_ros.py"
+
+    pid_value="$(ps -e | grep $pattern | grep -v grep | awk '{ print $1 }')"
+    if [ "$pid_value" != "" ]; then
+        pid=$pid_value
+        echo "Send SIGINT to pid $pid"
+    else
+        pid=1
+        echo "Pattern not found, send SIGINT to pid $pid"
+    fi
+    kill -s SIGINT $pid
+}
+# Use SIGTERM or TERM, does not seem to make any difference.
+trap _term TERM
+
+ros-with-env ros2 launch ntrip_client ntrip_client_launch.py &
+child=$!
+
+echo "Waiting for pid $child"
+# * Calling "wait" will then wait for the job with the specified by $child to finish, or for any signals to be fired.
+#   Due to "or for any signals to be fired", "wait" will also handle SIGTERM and it will shutdown before
+#   the node ends gracefully.
+#   The solution is to add a second "wait" call and remove the trap between the two calls.
+# * Do not use -e flag in the first wait call because wait will exit with error after catching SIGTERM.
+set +e
+wait $child
+set -e
+trap - TERM
+wait $child
+RESULT=$?
+
+if [ $RESULT -ne 0 ]; then
+    echo "ERROR: ntrip node failed with code $RESULT" >&2
+    exit $RESULT
+else
+    echo "INFO: ntrip node finished successfully, but returning 125 code for docker to restart properly." >&2
+    exit 125
+fi

scripts/ntrip_ros.py

@@ -30,6 +30,21 @@
   have_rtcm_msgs = True
   from rtcm_msgs.msg import Message as rtcm_msgs_RTCM
 
+
+rtcm_message_types = {
+  1005: '1005 - Stationary RTK reference station ARP',
+  1074: '1074 - GPS MSM4',
+  1077: '1077 - GPS MSM7',
+  1084: '1084 - GLONASS MSM4',
+  1087: '1087 - GLONASS MSM7',
+  1094: '1094 - Galileo MSM4',
+  1097: '1097 - Galileo MSM7',
+  1124: '1124 - BeiDou MSM4',
+  1127: '1127 - BeiDou MSM7',
+  1230: '1230 - GLONASS code-phase biases',
+  4072: '4072 - Reference station PVT (u-blox proprietary RTCM message)',
+}
+
 class NTRIPRos(Node):
   def __init__(self):
     # Read a debug flag from the environment that should have been set by the launch file
@@ -103,6 +118,7 @@ def __init__(self):
     self._rtcm_pub = self.create_publisher(self._rtcm_message_type, '/fmu/in/GpsInjectData', 1000)
     # self._rtcm_pub = self.create_publisher(self._rtcm_message_type, 'rtcm', 1000)
 
+    self.get_logger().warning('Workaround print so we get a connection to RTK server.')
 
     # Initialize the client
     self._client = NTRIPClient(
@@ -168,13 +184,18 @@ def publish_rtcm(self):
 
       rtcm = np.frombuffer(raw_rtcm, dtype=np.uint8)
       len_rtcm = len(rtcm)
-      self.get_logger().info(' package length {}'.format(len_rtcm))    
 
-      if (len_rtcm > 255):  # Even though the message can have size of 300, len must be between [0,255]
-        self.get_logger().info('  Dropping....')   
+      rtcm_msg_len = 256 * rtcm[1] + rtcm[2]
+      rtcm_msg_no = int((256 * rtcm[3] + rtcm[4]) / 16)
+      msg_type_description = rtcm_message_types.get(rtcm_msg_no, f'Unknown RTCM message type: {rtcm_msg_no}')
+
+      self.get_logger().info(' package length {:3} ({:3}), Type: {}'.format(len_rtcm, rtcm_msg_len, msg_type_description))
+
+      if (len_rtcm > MAX_LEN):  # Even though the message can have size of 300, len must be between [0,255]
+        self.get_logger().info('  Dropping....')
         continue
 
-      if (len_rtcm < MAX_LEN):
+      if (len_rtcm <= MAX_LEN):
         extend_array = np.zeros(MAX_LEN)
         rtcm = np.append(rtcm,extend_array)    
 
@@ -188,6 +209,7 @@ def publish_rtcm(self):
         self._rtcm_pub.publish(msg)
 
       else:
+        # not used as dropping the messages with length > 300
         fragmentId = 0
         start = 0
         while (start < len_rtcm):
@@ -216,7 +238,9 @@ def publish_rtcm(self):
 
           start += length
 
-      self._sequenceId += 1 
+      self._sequenceId += 1
+      if self._sequenceId > 31:
+        self._sequenceId = 0
 
 
 if __name__ == '__main__':