diff --git a/config/nmea_serial_driver.yaml b/config/nmea_serial_driver.yaml index 1e3b55d..3769876 100644 --- a/config/nmea_serial_driver.yaml +++ b/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 \ No newline at end of file diff --git a/config/nmea_tcpclient_driver.yaml b/config/nmea_tcpclient_driver.yaml index c7a3141..4cb9b77 100644 --- a/config/nmea_tcpclient_driver.yaml +++ b/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 \ No newline at end of file diff --git a/src/libnmea_navsat_driver/driver.py b/src/libnmea_navsat_driver/driver.py index c787833..704811c 100644 --- a/src/libnmea_navsat_driver/driver.py +++ b/src/libnmea_navsat_driver/driver.py @@ -35,12 +35,55 @@ 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): @@ -48,6 +91,19 @@ def __init__(self): 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 diff --git a/src/libnmea_navsat_driver/parser.py b/src/libnmea_navsat_driver/parser.py index 722c90e..131362e 100644 --- a/src/libnmea_navsat_driver/parser.py +++ b/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) + ] }