Fix/gpgga parse error

config/nmea_serial_driver.yaml

@@ -1,7 +1,7 @@
 nmea_navsat_driver:
   ros__parameters:
-    port: "/dev/tty.usbserial"
-    baud: 4800
+    port: "/dev/ttyS0"
+    baud: 9600
     frame_id: "gps"
     time_ref_source: "gps"
     useRMC: False

config/nmea_tcpclient_driver.yaml

@@ -1,5 +1,5 @@
 nmea_navsat_driver:
   ros__parameters:
-    ip: "192.168.131.22"
-    port: 9001
+    ip: "192.168.1.111"
+    port: 9904
     buffer_size: 4096

src/libnmea_navsat_driver/driver.py

@@ -35,19 +35,75 @@
 import rclpy
 
 from rclpy.node import Node
-from sensor_msgs.msg import NavSatFix, NavSatStatus, TimeReference
+from sensor_msgs.msg import NavSatFix, NavSatStatus, TimeReference, Imu
 from geometry_msgs.msg import TwistStamped, QuaternionStamped
+from ublox_msgs.msg import NavPVT
+from autoware_sensing_msgs.msg import GnssInsOrientationStamped
+
 from tf_transformations import quaternion_from_euler
 from libnmea_navsat_driver.checksum_utils import check_nmea_checksum
 from libnmea_navsat_driver import parser
 
+import math
+import numpy as np
+from std_msgs.msg import Float32
+
+def eulerFromQuaternion(x, y, z, w):
+  t0 = +2.0 * (w * x + y * z)
+  t1 = +1.0 - 2.0 * (x * x + y * y)
+  roll_x = math.atan2(t0, t1)
+
+  t2 = +2.0 * (w * y - z * x)
+  t2 = +1.0 if t2 > +1.0 else t2
+  t2 = -1.0 if t2 < -1.0 else t2
+  pitch_y = math.asin(t2)
+
+  t3 = +2.0 * (w * z + x * y)
+  t4 = +1.0 - 2.0 * (y * y + z * z)
+  yaw_z = math.atan2(t3, t4)
+
+  return roll_x, pitch_y, yaw_z
+
+def get_quaternion_from_euler(roll, pitch, yaw):
+  """
+  Convert an Euler angle to a quaternion.
+
+  Input
+    :param roll: The roll (rotation around x-axis) angle in radians.
+    :param pitch: The pitch (rotation around y-axis) angle in radians.
+    :param yaw: The yaw (rotation around z-axis) angle in radians.
+
+  Output
+    :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format
+  """
+  qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
+  qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
+  qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
+  qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
+
+  return [qx, qy, qz, qw]
+
+
 
 class Ros2NMEADriver(Node):
     def __init__(self):
         super().__init__('nmea_navsat_driver')
 
         self.fix_pub = self.create_publisher(NavSatFix, 'fix', 10)
         self.vel_pub = self.create_publisher(TwistStamped, 'vel', 10)
+
+        self.ublox_navpvt_pub = self.create_publisher(NavPVT, "navpvt", 10)
+        self.imu_pub = self.create_publisher(Imu, 'chc/imu', 10)
+        self.pub_pitch = self.create_publisher(Float32, 'chc/pitch', 2)
+        self.pub_heading = self.create_publisher(Float32, 'chc/heading', 2)
+        self.pub_orientation = self.create_publisher(GnssInsOrientationStamped, '/autoware_orientation', 2)
+        self.data = []
+        self.pitch_msg = Float32()
+        self.heading_msg = Float32()
+        self.imu_msg = Imu()
+        self.orientation_msg = GnssInsOrientationStamped()
+
+
         self.heading_pub = self.create_publisher(QuaternionStamped, 'heading', 10)
         self.time_ref_pub = self.create_publisher(TimeReference, 'time_reference', 10)
 
@@ -269,6 +325,55 @@ def add_sentence(self, nmea_string, frame_id, timestamp=None):
                 current_heading.quaternion.z = q[2]
                 current_heading.quaternion.w = q[3]
                 self.heading_pub.publish(current_heading)
+        elif 'CHC' in parsed_sentence:
+            data = parsed_sentence['CHC']
+            if data['fix_valid']==4:
+                navpvt_msg = NavPVT()
+                navpvt_msg.heading = int(math.degrees(data['heading'])*100000)
+                self.ublox_navpvt_pub.publish(navpvt_msg)
+
+            try:
+                self.pitch_msg.data = data["pitch"]
+                self.data.append(data["pitch"])
+                self.pub_pitch.publish(self.pitch_msg)
+
+                self.heading_msg.data = data['heading']
+                self.pub_heading.publish(self.heading_msg)
+
+                self.imu_msg.header.stamp = self.get_clock().now().to_msg()
+                self.imu_msg.header.frame_id = "gnss"
+                # orientation
+                heading = math.radians(90.0-data['heading'])
+
+
+                pitch = math.radians(data['pitch'])
+                roll = math.radians(data['roll'])
+                [qx, qy, qz, qw] = get_quaternion_from_euler(roll, pitch, heading)
+                self.imu_msg.orientation.x = qx
+                self.imu_msg.orientation.y = qy
+                self.imu_msg.orientation.z = qz
+                self.imu_msg.orientation.w = qw
+                # angular velocity the coordinate of imu is y-front x-right z-up, 
+                # so it has to be converted to right-handed coordinate
+                self.imu_msg.angular_velocity.x = math.radians(data["angular_velocity_y"])
+                self.imu_msg.angular_velocity.y =  math.radians(-data["angular_velocity_x"])
+                self.imu_msg.angular_velocity.z =  math.radians(data["angular_velocity_z"])
+                self.imu_msg.angular_velocity_covariance[0] = 0.001
+                self.imu_msg.angular_velocity_covariance[4] = 0.001
+                self.imu_msg.angular_velocity_covariance[8] = 0.001
+                self.imu_msg.linear_acceleration.x = data["linear_acceleration_y"] * 9.80665
+                self.imu_msg.linear_acceleration.y = -data["linear_acceleration_x"]* 9.80665
+                self.imu_msg.linear_acceleration.z = data["linear_acceleration_z"]* 9.80665
+                self.imu_pub.publish(self.imu_msg) 
+                # orientation msg of autoware
+                self.orientation_msg.header = self.imu_msg.header
+                self.orientation_msg.orientation.orientation = self.imu_msg.orientation
+                self.orientation_msg.orientation.rmse_rotation_x = 0.001745329 # 0.1 degree
+                self.orientation_msg.orientation.rmse_rotation_y = 0.001745329 # 0.1 degree 
+                self.orientation_msg.orientation.rmse_rotation_z = 0.001745329 # 0.1 degree
+                self.pub_orientation.publish(self.orientation_msg)
+            except UnicodeDecodeError as err:
+                self.get_logger().warn("UnicodeDecodeError: {0}".format(err))
         else:
             return False
         return True

src/libnmea_navsat_driver/parser.py

@@ -58,7 +58,11 @@ def convert_latitude(field):
 
 
 def convert_longitude(field):
-    return safe_float(field[0:3]) + safe_float(field[3:]) / 60.0
+    degree = safe_float(field[-len(field):-11])
+    minute = safe_float(field[-11:-1])
+    if(degree < 0):
+        minute = -minute
+    return degree + minute / 60.0
 
 
 def convert_time(nmea_utc):
@@ -139,7 +143,21 @@ def convert_deg_to_rads(degs):
     "VTG": [
         ("true_course", safe_float, 1),
         ("speed", convert_knots_to_mps, 5)
-    ]
+    ],
+    "CHC": [
+        ("gps_week", int, 1),
+        ("gps_second", safe_float, 2),
+        ("heading", safe_float, 3),
+        ("pitch", safe_float, 4),
+        ("roll", safe_float, 5),
+        ("angular_velocity_x", safe_float, 6),
+        ("angular_velocity_y", safe_float, 7),
+        ("angular_velocity_z", safe_float, 8),
+        ("linear_acceleration_x", safe_float, 9),
+        ("linear_acceleration_y", safe_float, 10),
+        ("linear_acceleration_z", safe_float, 11),
+        ("fix_valid", int, 21)
+        ]
 }