From 88f667ba2fa53d9bd7c5269d13dd85d7ad03fccb Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 10:22:26 +0000 Subject: [PATCH 1/4] Delete livox and replace with velodyne. --- .../launch/data_processing.launch.xml | 31 ++++++------------- orange_bringup/launch/orange_robot.launch.xml | 14 ++++----- 2 files changed, 16 insertions(+), 29 deletions(-) diff --git a/orange_bringup/launch/data_processing.launch.xml b/orange_bringup/launch/data_processing.launch.xml index abef442..31a414f 100644 --- a/orange_bringup/launch/data_processing.launch.xml +++ b/orange_bringup/launch/data_processing.launch.xml @@ -7,8 +7,6 @@ - - @@ -33,35 +31,24 @@ - - - - - - - + + + - - - - - - + + diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index 624a90d..32c4dea 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -62,7 +62,7 @@ - - + + @@ -94,7 +94,7 @@ --> - + - + From 4eca6d6c7cbf2eddbe0d22d88b69ddb9baf9d0c9 Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 11:08:22 +0000 Subject: [PATCH 2/4] Migrate GNSS-related programs to orange_gnss. --- orange_bringup/orange_bringup/get_lonlat.py | 152 ------ orange_bringup/setup.py | 10 +- .../orange_gnss}/GPSodom_correction.py | 0 orange_gnss/orange_gnss/__init__.py | 0 .../orange_gnss}/combination.py | 2 +- .../orange_gnss}/ekf_myself.py | 111 +++-- .../orange_gnss}/ekf_myself_noGPS.py | 0 .../orange_gnss}/fix_to_GPSodom.py | 0 orange_gnss/orange_gnss/get_lonlat_ttyACM.py | 104 ++++ orange_gnss/orange_gnss/get_lonlat_ttyUSB.py | 134 +++++ .../orange_gnss/get_movingbase_quat_ttyUSB.py | 142 ++++++ ...ss_odom_movingbase_fix_publisher_ttyUSB.py | 460 ++++++++++++++++++ .../orange_gnss/gnss_odom_publisher_ttyUSB.py | 391 +++++++++++++++ .../orange_gnss}/lonlat_to_odom.py | 3 +- .../orange_gnss}/movingbase_yaw_to_quat.py | 9 +- orange_gnss/package.xml | 23 + orange_gnss/resource/orange_gnss | 0 orange_gnss/setup.cfg | 4 + orange_gnss/setup.py | 42 ++ 19 files changed, 1387 insertions(+), 200 deletions(-) delete mode 100755 orange_bringup/orange_bringup/get_lonlat.py rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/GPSodom_correction.py (100%) create mode 100644 orange_gnss/orange_gnss/__init__.py rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/combination.py (97%) rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/ekf_myself.py (80%) rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/ekf_myself_noGPS.py (100%) rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/fix_to_GPSodom.py (100%) create mode 100755 orange_gnss/orange_gnss/get_lonlat_ttyACM.py create mode 100644 orange_gnss/orange_gnss/get_lonlat_ttyUSB.py create mode 100644 orange_gnss/orange_gnss/get_movingbase_quat_ttyUSB.py create mode 100644 orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py create mode 100644 orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/lonlat_to_odom.py (98%) rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/movingbase_yaw_to_quat.py (94%) create mode 100644 orange_gnss/package.xml create mode 100644 orange_gnss/resource/orange_gnss create mode 100644 orange_gnss/setup.cfg create mode 100644 orange_gnss/setup.py diff --git a/orange_bringup/orange_bringup/get_lonlat.py b/orange_bringup/orange_bringup/get_lonlat.py deleted file mode 100755 index 1aa8417..0000000 --- a/orange_bringup/orange_bringup/get_lonlat.py +++ /dev/null @@ -1,152 +0,0 @@ -#!/usr/bin/env python3 -import math - -import rclpy -import serial -from nav_msgs.msg import Odometry -from rclpy.node import Node -from sensor_msgs.msg import NavSatFix, NavSatStatus -from std_msgs.msg import Header - - -class GPSData(Node): - def __init__(self): - super().__init__('gps_data_acquisition') - - self.declare_parameter('port', '/dev/sensors/GNSSbase') - self.declare_parameter('baud', 9600) - self.declare_parameter('country_id', 0) - self.declare_parameter('type', 1) # 1=ttyACM 2=ttyUSB - - self.dev_name = self.get_parameter( - 'port').get_parameter_value().string_value - self.serial_baud = self.get_parameter( - 'baud').get_parameter_value().integer_value - self.country_id = self.get_parameter( - 'country_id').get_parameter_value().integer_value - self.type = self.get_parameter( - 'type').get_parameter_value().integer_value - - self.lonlat_pub = self.create_publisher(NavSatFix, "/fix", 1) - self.lonlat_msg = NavSatFix() - - self.initial_coordinate = None - self.fix_data = None - - self.timer = self.create_timer(1.0, self.publish_GPS_lonlat) - - self.get_logger().info("Start get_lonlat node") - self.get_logger().info("---------------------") - - def get_gps(self, dev_name, country_id): - try: - serial_port = serial.Serial(dev_name, self.serial_baud) - except serial.SerialException as serialerror: - self.get_logger().error(f"Serial error: {serialerror}") - return None - - initial_letters = None - if country_id == 0: # Japan - initial_letters = "$GNGGA" - elif country_id == 1: # USA - initial_letters = "$GPGGA" - - if self.type == 1: - while True: - line = serial_port.readline().decode('latin-1') - gps_data = line.split(',') - if gps_data[0] == initial_letters: - break - - Fixtype_data = int(gps_data[6]) - if Fixtype_data != 0: - satelitecount_data = float(gps_data[7]) - # ddmm.mmmmm to dd.ddddd - latitude_data = float(gps_data[2]) / 100.0 - if gps_data[3] == 'S': # south - latitude_data *= -1 - # ddmm.mmmmm to dd.ddddd - longitude_data = float(gps_data[4]) / 100.0 - if gps_data[5] == 'W': # west - longitude_data *= -1 - altitude_data = float(gps_data[9]) - else: - latitude_data = 0 - longitude_data = 0 - altitude_data = 0 - satelitecount_data = 0 - - elif self.type == 2: - line = serial_port.readline() - talker_ID = line.find(initial_letters) - if talker_ID != -1: - line = line[(talker_ID-1):] - gps_data = line.split(b",") - rospy.loginfo(gps_data) - Fixtype_data = int(gps_data[6]) - rospy.loginfo(Fixtype_data) - if Fixtype_data != 0: - satelitecount_data = int(gps_data[7]) - rospy.loginfo(satelitecount_data) - if Fixtype_data != 0: - # ddmm.mmmmm to dd.ddddd - latitude_data = float(gps_data[2]) / 100.0 - if gps_data[3] == b"S": # south - latitude_data *= -1 - # ddmm.mmmmm to dd.ddddd - longitude_data = float(gps_data[4]) / 100.0 - if gps_data[5] == b"W": # west - longitude_data *= -1 - altitude_data = float(gps_data[9]) - else: - # not fix data - latitude_data = 0 - longitude_data = 0 - altitude_data = 0 - satelitecount_data = 0 - else: - # no GPS data - latitude_data = 0 - longitude_data = 0 - altitude_data = 0 - satelitecount_data = 0 - else: - rospy.loginfo( - "current latitude and longitude (Fixtype,latitude, longitude,altitude):None") - return None - - serial_port.close() - gnggadata = (Fixtype_data, latitude_data, longitude_data, - altitude_data, satelitecount_data) - - return gnggadata - - def publish_GPS_lonlat(self): - lonlat = self.get_gps(self.dev_name, self.country_id) - if lonlat: - self.lonlat_msg.header = Header() - self.lonlat_msg.header.frame_id = "gps" - self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() - - self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if lonlat[ - 0] != 0 else NavSatStatus.STATUS_NO_FIX - self.lonlat_msg.latitude = float(lonlat[1]) - self.lonlat_msg.longitude = float(lonlat[2]) - self.lonlat_msg.altitude = float(lonlat[3]) - - self.lonlat_pub.publish(self.lonlat_msg) - # self.get_logger().info(f"Published GPS data: {lonlat}") - else: - self.get_logger().error("!!! -gps data error- !!!") - - -def main(args=None): - rclpy.init(args=args) - gpslonlat = GPSData() - rclpy.spin(gpslonlat) - gpslonlat.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/orange_bringup/setup.py b/orange_bringup/setup.py index 1e3ca3c..10c00b8 100644 --- a/orange_bringup/setup.py +++ b/orange_bringup/setup.py @@ -29,15 +29,7 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "motor_driver_node = orange_bringup.motor_driver_node:main", - "fix_to_GPSodom = orange_bringup.fix_to_GPSodom:main", - "movingbase_yaw_to_quat = orange_bringup.movingbase_yaw_to_quat:main", - "combination = orange_bringup.combination:main", - "ekf_myself = orange_bringup.ekf_myself:main", - "get_lonlat = orange_bringup.get_lonlat:main", - "GPSodom_correction = orange_bringup.GPSodom_correction:main", - "lonlat_to_odom = orange_bringup.lonlat_to_odom:main", - "ekf_myself_noGPS = orange_bringup.ekf_myself_noGPS:main" + "motor_driver_node = orange_bringup.motor_driver_node:main" ], }, ) diff --git a/orange_bringup/orange_bringup/GPSodom_correction.py b/orange_gnss/orange_gnss/GPSodom_correction.py similarity index 100% rename from orange_bringup/orange_bringup/GPSodom_correction.py rename to orange_gnss/orange_gnss/GPSodom_correction.py diff --git a/orange_gnss/orange_gnss/__init__.py b/orange_gnss/orange_gnss/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/orange_bringup/orange_bringup/combination.py b/orange_gnss/orange_gnss/combination.py similarity index 97% rename from orange_bringup/orange_bringup/combination.py rename to orange_gnss/orange_gnss/combination.py index f8e8e21..1cd3900 100644 --- a/orange_bringup/orange_bringup/combination.py +++ b/orange_gnss/orange_gnss/combination.py @@ -28,7 +28,7 @@ def __init__(self): self.timer = self.create_timer(1.0, self.publish_combined_odom) self.get_logger().info("Start combination node") - self.get_logger().info("----------------------") + self.get_logger().info("-------------------------") def odomgps_callback(self, msg): self.x = msg.pose.pose.position.x diff --git a/orange_bringup/orange_bringup/ekf_myself.py b/orange_gnss/orange_gnss/ekf_myself.py similarity index 80% rename from orange_bringup/orange_bringup/ekf_myself.py rename to orange_gnss/orange_gnss/ekf_myself.py index 8da39ef..50d3b99 100644 --- a/orange_bringup/orange_bringup/ekf_myself.py +++ b/orange_gnss/orange_gnss/ekf_myself.py @@ -43,13 +43,20 @@ def __init__(self): self.robot_orientationz = 0 self.robot_orientationw = 0 self.Number_of_satellites = 0 + + ###### angle offset ####### + self.GPS_angle_conut = 0 + self.GPS_angle_offset = 0 + self.sub_a = self.create_subscription( - Odometry, '/odom_fast', self.sensor_a_callback, 10) + Odometry, '/odom', self.sensor_a_callback, 10) self.sub_b = self.create_subscription( - Odometry, '/odom_CLAS_movingbase', self.sensor_b_callback, 10) + Odometry, '/odom/UM982', self.sensor_b_callback, 10) + #self.sub_b = self.create_subscription( + # Odometry, '/odom_ref_slam', self.sensor_b_callback, 10) - self.declare_parameter("ekf_publish_TF", True) + self.declare_parameter("ekf_publish_TF", False) self.ekf_publish_TF = self.get_parameter( "ekf_publish_TF").get_parameter_value().bool_value @@ -62,7 +69,7 @@ def __init__(self): self.timer = self.create_timer(0.1, self.publish_fused_value) self.get_logger().info("Start ekf_myself node") - self.get_logger().info("---------------------") + self.get_logger().info("-------------------------") def orientation_to_yaw(self, z, w): yaw = np.arctan2(2.0 * (w * z), 1.0 - 2.0 * (z ** 2)) @@ -83,17 +90,21 @@ def sensor_a_callback(self, data): self.SmpTime = current_time - self.prev_time else: self.SmpTime = 0.1 - self.prev_time = current_time + self.prev_time = current_time + + current_pos = np.array([ data.pose.pose.position.x, data.pose.pose.position.y ]) + if self.prev_pos is not None: distance = np.linalg.norm(current_pos - self.prev_pos) self.Speed = distance / self.SmpTime else: self.Speed = 0 + self.prev_pos = current_pos self.GTheta = self.orientation_to_yaw( @@ -102,6 +113,12 @@ def sensor_a_callback(self, data): def sensor_b_callback(self, data): self.GpsXY = np.array( [data.pose.pose.position.x, data.pose.pose.position.y]) + #self.GpsXY = np.array( + # [data.pose.pose.position.x, data.pose.pose.position.y, 0]) + #pointcloud, rot_matrix = rotation_xyz(self.GpsXY, 0, 0, 90) + #self.GpsXY = np.array( + # [pointcloud[0], pointcloud[1]]) + self.GPStheta = self.orientation_to_yaw( data.pose.pose.orientation.z, data.pose.pose.orientation.w) @@ -111,23 +128,27 @@ def sensor_b_callback(self, data): self.GPSthetayaw0 = self.GPStheta self.Number_of_satellites = data.pose.covariance[0] # + + + + + # self.get_logger().info(f"self.Number_of_satellites: {self.Number_of_satellites}") def determination_of_R(self): if 0 <= self.Number_of_satellites < 4: # Bad - self.R1 = 1e-2 # 0.01 FAST-LIO - self.R2 = 9e-2 # 0.09 CLAS-movingbase + self.R1 = 0.17**2 # FAST-LIO + self.R2 = 0.17**2 # CLAS-movingbase self.R3 = 9 # GTheta self.R4 = 1 # GPStheta - - elif 4 <= self.Number_of_satellites < 8: # So-so - self.R1 = 6e-2 # 0.06 FAST-LIO - self.R2 = 4e-2 # 0.04 CLAS-movingbase + if 4 <= self.Number_of_satellites < 8: # So-so... + self.R1 = 0.08**2 # FAST-LIO + self.R2 = 0.08**2 # CLAS-movingbase self.R3 = 4 # GTheta self.R4 = 6 # GPStheta elif self.Number_of_satellites >= 8: # Good!!! - self.R1 = 9e-2 # 0.09 FAST-LIO - self.R2 = 1e-2 # 0.01 CLAS-movingbase + self.R1 = 0.05**2 # FAST-LIO + self.R2 = 0.05**2 # CLAS-movingbase self.R3 = 2 # GTheta self.R4 = 8 # GPStheta @@ -245,37 +266,44 @@ def combine_yaw(self, Dtheta, theta1, theta2, w1, w2): return theta_sum def calculate_offset(self, combyaw, GTheta, GPStheta): - deference = abs(GTheta) + abs(GPStheta) + abs_GTheta = abs(GTheta) + abs_GPStheta = abs(GPStheta) + abs_combyaw = abs(combyaw) + pi = math.pi + + deference = abs_GTheta + abs_GPStheta if GTheta > 0 and GPStheta < 0 and combyaw > 0: self.GOffset = -(GTheta - combyaw) elif GTheta > 0 and GPStheta < 0 and combyaw < 0: - self.GOffset = -(GTheta + abs(combyaw)) + self.GOffset = -(GTheta + abs_combyaw) elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta > GPStheta: - self.GOffset = -(abs(combyaw) - abs(GTheta)) + self.GOffset = -(abs_combyaw - abs_GTheta) elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta > GPStheta: - self.GOffset = -(GTheta + abs(combyaw)) + self.GOffset = -(GTheta + abs_combyaw) elif GTheta < 0 and GPStheta > 0 and combyaw > 0: - self.GOffset = abs(GTheta) + combyaw + self.GOffset = abs_GTheta + combyaw elif GTheta < 0 and GPStheta > 0 and combyaw < 0: - self.GOffset = abs(GTheta) - abs(combyaw) + self.GOffset = abs_GTheta - abs_combyaw elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta < GPStheta: self.GOffset = combyaw - GTheta elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta < GPStheta: - self.GOffset = abs(GTheta) - abs(combyaw) - elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > math.pi: + self.GOffset = abs_GTheta - abs_combyaw + elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > pi: self.GOffset = combyaw - GTheta - elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > math.pi: - self.GOffset = math.pi - GTheta + math.pi - abs(combyaw) - elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > math.pi: - self.GOffset = -((math.pi - combyaw) + (math.pi - abs(GTheta))) - elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > math.pi: - self.GOffset = -(abs(combyaw) - abs(GTheta)) - - if abs(self.GOffset) > 5 * math.pi / 180: # not -0.0872 ~ 0.0872 + elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > pi: + self.GOffset = pi - GTheta + pi - abs_combyaw + elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > pi: + self.GOffset = -((pi - combyaw) + (pi - abs_GTheta)) + elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > pi: + self.GOffset = -(abs_combyaw - abs_GTheta) + + if abs(self.GOffset) > 5 * pi / 180: # not -0.0872 ~ 0.0872 self.GOffset = 0 self.get_logger().warn("GOffset warning") + # self.get_logger().info(f"GOffset: {self.GOffset}") ok + return self.GOffset def publish_fused_value(self): @@ -285,12 +313,11 @@ def publish_fused_value(self): self.R2 = R[1] self.R3 = R[2] self.R4 = R[3] - - if self.GpsXY is not None: + if self.GpsXY is not None : fused_value = self.KalfGPSXY( self.Speed, self.SmpTime, self.GTheta, self.GpsXY, self.R1, self.R2) self.GPS_conut += 1 - if self.GPS_conut % 10 == 0: + if self.GPS_conut % 20 == 0: self.combyaw = self.combine_yaw( self.DGPStheta, self.GTheta, self.GPStheta, self.R3, self.R4) self.offsetyaw = self.calculate_offset( @@ -354,6 +381,26 @@ def publish_fused_value(self): self.t.transform.rotation.w = float(self.robot_orientationw) self.br.sendTransform(self.t) +def rotation_xyz(pointcloud, theta_x, theta_y, theta_z): + rad_x = math.radians(theta_x) + rad_y = math.radians(theta_y) + rad_z = math.radians(theta_z) + rot_x = np.array([[ 1, 0, 0], + [ 0, math.cos(rad_x), -math.sin(rad_x)], + [ 0, math.sin(rad_x), math.cos(rad_x)]]) + + rot_y = np.array([[ math.cos(rad_y), 0, math.sin(rad_y)], + [ 0, 1, 0], + [-math.sin(rad_y), 0, math.cos(rad_y)]]) + + rot_z = np.array([[ math.cos(rad_z), -math.sin(rad_z), 0], + [ math.sin(rad_z), math.cos(rad_z), 0], + [ 0, 0, 1]]) + rot_matrix = rot_z.dot(rot_y.dot(rot_x)) + #print(f"rot_matrix ={rot_matrix}") + #print(f"pointcloud ={pointcloud.shape}") + rot_pointcloud = rot_matrix.dot(pointcloud) + return rot_pointcloud, rot_matrix def main(args=None): rclpy.init(args=args) diff --git a/orange_bringup/orange_bringup/ekf_myself_noGPS.py b/orange_gnss/orange_gnss/ekf_myself_noGPS.py similarity index 100% rename from orange_bringup/orange_bringup/ekf_myself_noGPS.py rename to orange_gnss/orange_gnss/ekf_myself_noGPS.py diff --git a/orange_bringup/orange_bringup/fix_to_GPSodom.py b/orange_gnss/orange_gnss/fix_to_GPSodom.py similarity index 100% rename from orange_bringup/orange_bringup/fix_to_GPSodom.py rename to orange_gnss/orange_gnss/fix_to_GPSodom.py diff --git a/orange_gnss/orange_gnss/get_lonlat_ttyACM.py b/orange_gnss/orange_gnss/get_lonlat_ttyACM.py new file mode 100755 index 0000000..dd72e9f --- /dev/null +++ b/orange_gnss/orange_gnss/get_lonlat_ttyACM.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +import rclpy +import serial +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix, NavSatStatus +from std_msgs.msg import Header + + +class GPSData(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + self.declare_parameter('port', '/dev/sensors/GNSSbase') + self.declare_parameter('baud', 9600) + self.declare_parameter('country_id', 0) + + self.dev_name = self.get_parameter( + 'port').get_parameter_value().string_value + self.serial_baud = self.get_parameter( + 'baud').get_parameter_value().integer_value + self.country_id = self.get_parameter( + 'country_id').get_parameter_value().integer_value + + self.lonlat_pub = self.create_publisher(NavSatFix, "/fix", 1) + self.lonlat_msg = NavSatFix() + + self.initial_coordinate = None + self.fix_data = None + + self.timer = self.create_timer(1.0, self.publish_GPS_lonlat) + + self.get_logger().info("Start get_lonlat node") + self.get_logger().info("-------------------------") + + def get_gps(self, dev_name, country_id): + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + initial_letters = None + if country_id == 0: # Japan + initial_letters = "$GNGGA" + elif country_id == 1: # USA + initial_letters = "$GPGGA" + + while True: + line = serial_port.readline().decode('latin-1') + gps_data = line.split(',') + if gps_data[0] == initial_letters: + break + + Fixtype_data = int(gps_data[6]) + if Fixtype_data != 0: + satelitecount_data = float(gps_data[7]) + # ddmm.mmmmm to dd.ddddd + latitude_data = float(gps_data[2]) / 100.0 + if gps_data[3] == 'S': # south + latitude_data *= -1 + # ddmm.mmmmm to dd.ddddd + longitude_data = float(gps_data[4]) / 100.0 + if gps_data[5] == 'W': # west + longitude_data *= -1 + altitude_data = float(gps_data[9]) + else: + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + + serial_port.close() + gnggadata = (Fixtype_data, latitude_data, longitude_data,altitude_data, satelitecount_data) + + return gnggadata + + def publish_GPS_lonlat(self): + lonlat = self.get_gps(self.dev_name, self.country_id) + if lonlat: + self.lonlat_msg.header = Header() + self.lonlat_msg.header.frame_id = "gps" + self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() + + self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if lonlat[ + 0] != 0 else NavSatStatus.STATUS_NO_FIX + self.lonlat_msg.latitude = float(lonlat[1]) + self.lonlat_msg.longitude = float(lonlat[2]) + self.lonlat_msg.altitude = float(lonlat[3]) + + self.lonlat_pub.publish(self.lonlat_msg) + # self.get_logger().info(f"Published GPS data: {lonlat}") + else: + self.get_logger().error("!!!!-gps data error-!!!!") + + +def main(args=None): + rclpy.init(args=args) + gpslonlat = GPSData() + rclpy.spin(gpslonlat) + gpslonlat.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/orange_gnss/orange_gnss/get_lonlat_ttyUSB.py b/orange_gnss/orange_gnss/get_lonlat_ttyUSB.py new file mode 100644 index 0000000..4004cbe --- /dev/null +++ b/orange_gnss/orange_gnss/get_lonlat_ttyUSB.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 +import rclpy +import serial +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix, NavSatStatus +from std_msgs.msg import Header + + +class GPSData(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + self.declare_parameter('port', '/dev/sensors/GNSS_UM982') + self.declare_parameter('baud', 115200) + self.declare_parameter('country_id', 0) + + self.dev_name = self.get_parameter( + 'port').get_parameter_value().string_value + self.serial_baud = self.get_parameter( + 'baud').get_parameter_value().integer_value + self.country_id = self.get_parameter( + 'country_id').get_parameter_value().integer_value + + self.lonlat_pub = self.create_publisher(NavSatFix, "/fix", 1) + self.lonlat_msg = NavSatFix() + + self.initial_coordinate = None + self.fix_data = None + + self.timer = self.create_timer(1.0, self.publish_GPS_lonlat) + + self.get_logger().info("Start get_lonlat node") + self.get_logger().info("-------------------------") + + def get_gps(self, dev_name, country_id): + # interface with sensor device(as a serial port) + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + # country info + if country_id == 0: # Japan + initial_letters = b"GNGGA" + elif country_id == 1: # USA + initial_letters = b"GPGGA" + else: # not certain + initial_letters = None + +# gps_data = ["$G?GGA", +# "UTC time", +# "Latitude (ddmm.mmmmm)", +# "latitude type (south/north)", +# "Longitude (ddmm.mmmmm)", +# "longitude type (east longitude/west longitude)", +# "Fixtype", +# "Number of satellites used for positioning", +# "HDOP", +# "Altitude", +# "M(meter)", +# "Elevation", +# "M(meter)", +# "", +# "checksum"] + + line = serial_port.readline() + talker_ID = line.find(initial_letters) + if talker_ID != -1: + line = line[(talker_ID-1):] + gps_data = line.split(b",") + Fixtype_data = int(gps_data[6]) + if Fixtype_data != 0: + satelitecount_data = int(gps_data[7])### + if Fixtype_data != 0: + latitude_data = float(gps_data[2]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[3] == b"S":#south + latitude_data *= -1 + longitude_data = float(gps_data[4]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[5] == b"W":#west + longitude_data *= -1 + altitude_data = float(gps_data[9]) + else : + #not fix data + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not fix data--!") + else : + #no GPS data + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not GPS data--!") + else: + self.get_logger().error("!--not GPS data--!") + return None + serial_port.close() + + gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data) + return gnggadata + + + def publish_GPS_lonlat(self): + lonlat = self.get_gps(self.dev_name, self.country_id) + if lonlat: + self.lonlat_msg.header = Header() + self.lonlat_msg.header.frame_id = "gps" + self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() + + self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if lonlat[ + 0] != 0 else NavSatStatus.STATUS_NO_FIX + self.lonlat_msg.latitude = float(lonlat[1]) + self.lonlat_msg.longitude = float(lonlat[2]) + self.lonlat_msg.altitude = float(lonlat[3]) + + self.lonlat_pub.publish(self.lonlat_msg) + # self.get_logger().info(f"Published GPS data: {lonlat}") + else: + self.get_logger().error("!!!!-gps data error-!!!!") + + +def main(args=None): + rclpy.init(args=args) + gpslonlat = GPSData() + rclpy.spin(gpslonlat) + gpslonlat.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/orange_gnss/orange_gnss/get_movingbase_quat_ttyUSB.py b/orange_gnss/orange_gnss/get_movingbase_quat_ttyUSB.py new file mode 100644 index 0000000..ca65b5b --- /dev/null +++ b/orange_gnss/orange_gnss/get_movingbase_quat_ttyUSB.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 +import math +import rclpy +import serial +from rclpy.node import Node +from sensor_msgs.msg import Imu +from std_msgs.msg import Header + +class GPS_heading_Data(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + self.declare_parameter('port', '/dev/sensors/GNSS_UM982') + self.declare_parameter('baud', 115200) + + self.dev_name = self.get_parameter( + 'port').get_parameter_value().string_value + self.serial_baud = self.get_parameter( + 'baud').get_parameter_value().integer_value + + self.heading_pub = self.create_publisher(Imu, 'movingbase/quat', 10) + self.movingbase_msg = Imu() + + self.timer = self.create_timer(1.0, self.movingbase_publish_msg) + self.count = 0 + + self.get_logger().info("Start get_movingbase_quat_ttyUSB node") + self.get_logger().info("-------------------------") + + def get_gps_heading(self, dev_name): + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + initial_letters_outdoor = b"$GNHDT" + initial_letters_indoor = b"$GPHDT" + + while(1): + line = serial_port.readline() + #self.get_logger().info(f"line: {line}") + talker_ID_indoor = line.find(initial_letters_indoor) + talker_ID_outdoor = line.find(initial_letters_outdoor) + if talker_ID_indoor != -1: + #self.get_logger().info("GPHDT ok") + #line = line[(talker_ID_indoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + if talker_ID_outdoor != -1: + #self.get_logger().info("GNHDT ok") + #line = line[(talker_ID_outdoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + + serial_port.close() + + return heading + + def quaternion_from_euler(self, roll, pitch, yaw): + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + return q + + def movingbase_publish_msg(self): + real_heading = self.get_gps_heading(self.dev_name) + #self.get_logger().info(f"real_heading: {real_heading}") + if real_heading is not None and real_heading != 0: + + robotheading = real_heading + 90 + if robotheading >= 360: + robotheading -= 360 + + #self.get_logger().info(f"robotheading: {robotheading}") + + if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!") + self.first_heading = robotheading + self.count = 1 + + relative_heading = robotheading - self.first_heading + if relative_heading < 0: + relative_heading += 360 + + if relative_heading > 180: + relative_heading -= 360 + + movingbaseyaw = relative_heading * (math.pi / 180) + + roll, pitch = 0.0, 0.0 + yaw = movingbaseyaw + + q = self.quaternion_from_euler(roll, pitch, yaw) + # self.get_logger().info(f"Quaternion: {q}") + + self.movingbase_msg.header.stamp = self.get_clock().now().to_msg() + self.movingbase_msg.header.frame_id = "imu_link" + self.movingbase_msg.orientation.x = q[1] + self.movingbase_msg.orientation.y = q[2] + self.movingbase_msg.orientation.z = -q[3] # -z + self.movingbase_msg.orientation.w = q[0] + self.movingbase_msg.orientation_covariance[0] = robotheading + self.heading_pub.publish(self.movingbase_msg) + else: + self.movingbase_msg.header.stamp = self.get_clock().now().to_msg() + self.movingbase_msg.header.frame_id = "imu_link" + self.movingbase_msg.orientation.x = 0.0 + self.movingbase_msg.orientation.y = 0.0 + self.movingbase_msg.orientation.z = 0.0 + self.movingbase_msg.orientation.w = 0.0 + self.heading_pub.publish(self.movingbase_msg) + self.get_logger().error("!!!!-not movingbase data-!!!!") + +def main(args=None): + rclpy.init(args=args) + GPS_heading_Data_node = GPS_heading_Data() + rclpy.spin(GPS_heading_Data_node) + GPS_heading_Data_node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py new file mode 100644 index 0000000..9f8810c --- /dev/null +++ b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py @@ -0,0 +1,460 @@ +#!/usr/bin/env python3 +import math +import rclpy +import serial +import tkinter as tk +from rclpy.node import Node +from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus +from nav_msgs.msg import Odometry +from std_msgs.msg import Header +from geometry_msgs.msg import Quaternion, Pose, Point, Twist, Vector3 +import threading +import time +from my_msgs.srv import Avglatlon + +class GPSData(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + # Parameters + self.declare_parameter('port', '/dev/sensors/GNSS_UM982') + self.declare_parameter('baud', 115200) + self.declare_parameter('country_id', 0) + self.declare_parameter('Position_magnification', 1.675) + self.declare_parameter('heading', 90.0) + + self.dev_name = self.get_parameter('port').get_parameter_value().string_value + self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value + self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value + self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value + self.theta = self.get_parameter('heading').get_parameter_value().double_value + + #self.theta = 275.6 # tukuba param + #self.theta = 180 #nakaniwa param + self.initial_coordinate = None + self.fix_data = None + self.count = 0 + self.initialized = False # 平均初期座標が取得できたかどうか + + # Publishers + self.lonlat_pub = self.create_publisher(NavSatFix, '/fix', 1) + self.lonlat_msg = NavSatFix() + self.movingbase_pub = self.create_publisher(Imu, '/movingbase/quat', 10) + self.movingbase_msg = Imu() + self.odom_pub = self.create_publisher(Odometry, '/odom/UM982', 10) + self.odom_msg = Odometry() + + # service client + self.client = self.create_client(Avglatlon, 'send_avg_gps') + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available...") + + # Timers + self.timer = self.create_timer(1.0, self.timer_callback) + + self.first_heading = None + + self.gps_data_cache = None + + self.get_logger().info("Start get_lonlat_movingbase_quat_ttyUSB node") + self.get_logger().info("-------------------------") + + # tkinter GUI setup + self.root = tk.Tk() + self.root.title("GPS Data Acquisition") + self.start_button = tk.Button(self.root, text="Start", command=self.start_gps_acquisition, width=20, height = 5) + self.start_button.pack() + + self.gps_acquisition_thread = None + self.is_acquiring = False + + # service client + def send_request(self): + request = Avglatlon.Request() + request.avg_lat = self.initial_coordinate[0] # ← average lat + request.avg_lon = self.initial_coordinate[1] # ← average lon + request.theta = self.theta + + future = self.client.call_async(request) + future.add_done_callback(self.response_callback) + + def response_callback(self, future): + try: + response = future.result() + if response.success: + self.get_logger().info('サービス送信成功') + else: + self.get_logger().warn('サービスは受け取られましたが、処理は失敗しました') + except Exception as e: + self.get_logger().error(f'サービス呼び出し失敗: {e}') + + # timer callback + def timer_callback(self): + if not self.initialized: + # 初期化が完了していないので何もしない + return + self.gps_data_cache = self.get_gps_quat(self.dev_name, self.country_id) + + if self.gps_data_cache: + fix_type, lat, lon, alt, _, heading = self.gps_data_cache + if fix_type != 0: + self.publish_fix(self.gps_data_cache) + self.publish_odom(lat, lon, alt) + self.publish_movingbase(heading) + else: + self.get_logger().error("GPS not fixed") + else: + self.get_logger().error("Failed to get GPS data") + + # gps data collect + def start_gps_acquisition(self): + if not self.is_acquiring: + self.is_acquiring = True + self.gps_acquisition_thread = threading.Thread(target=self.acquire_gps_data) + self.gps_acquisition_thread.start() + + def acquire_gps_data(self): + lat_sum = 0.0 + lon_sum = 0.0 + count = 0 + + start_time = time.time() + while time.time() - start_time < 10: # 10 seconds + GPS_data = self.get_gps_quat(self.dev_name, self.country_id) + if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0: + lat_sum += GPS_data[1] + lon_sum += GPS_data[2] + count += 1 + time.sleep(0.1) # Slight delay to avoid overwhelming the GPS device + + if count > 0: + self.initial_coordinate = [lat_sum / count, lon_sum / count] + self.initialized = True + self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}") + self.send_request() + self.is_acquiring = False + + def get_gps_quat(self, dev_name, country_id): + # interface with sensor device(as a serial port) + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + # country info + if country_id == 0: # Japan + initial_letters = b"GNGGA" + elif country_id == 1: # USA + initial_letters = b"GPGGA" + else: # not certain + initial_letters = None + + initial_letters_outdoor = b"$GNHDT" + initial_letters_indoor = b"$GPHDT" + + while(1): + line = serial_port.readline() + #self.get_logger().info(f"line: {line}") + talker_ID_indoor = line.find(initial_letters_indoor) + talker_ID_outdoor = line.find(initial_letters_outdoor) + if talker_ID_indoor != -1: + #self.get_logger().info("GPHDT ok") + #line = line[(talker_ID_indoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + if talker_ID_outdoor != -1: + #self.get_logger().info("GNHDT ok") + #line = line[(talker_ID_outdoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + +# gps_data = ["$G?GGA", +# "UTC time", +# "Latitude (ddmm.mmmmm)", +# "latitude type (south/north)", +# "Longitude (ddmm.mmmmm)", +# "longitude type (east longitude/west longitude)", +# "Fixtype", +# "Number of satellites used for positioning", +# "HDOP", +# "Altitude", +# "M(meter)", +# "Elevation", +# "M(meter)", +# "", +# "checksum"] + + line = serial_port.readline() + talker_ID = line.find(initial_letters) + if talker_ID != -1: + line = line[(talker_ID-1):] + gps_data = line.split(b",") + Fixtype_data = int(gps_data[6]) + if Fixtype_data != 0: + satelitecount_data = int(gps_data[7])### + if Fixtype_data != 0: + latitude_data = float(gps_data[2]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[3] == b"S":#south + latitude_data *= -1 + longitude_data = float(gps_data[4]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[5] == b"W":#west + longitude_data *= -1 + altitude_data = float(gps_data[9]) + else : + #not fix data + Fixtype_data = 0 + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not fix data--!") + else : + #no GPS data + Fixtype_data = 0 + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not GPS data--!") + + serial_port.close() + + gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading) + return gnggadata + + def quaternion_from_euler(self, roll, pitch, yaw): + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + return q + + def heading_to_quat(self ,real_heading): + + robotheading = real_heading + 90 + if robotheading >= 360: + robotheading -= 360 + + #self.get_logger().info(f"real_heading: {real_heading}") + #self.get_logger().info(f"robotheading: {robotheading}") + + if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!") + self.first_heading = robotheading + self.count = 1 + + relative_heading = robotheading - self.first_heading + if relative_heading < 0: + relative_heading += 360 + + if relative_heading > 180: + relative_heading -= 360 + + movingbaseyaw = relative_heading * (math.pi / 180) + + roll, pitch = 0.0, 0.0 + yaw = movingbaseyaw + + q = self.quaternion_from_euler(roll, pitch, yaw) + # self.get_logger().info(f"Quaternion: {q}") + + return q + + def conversion(self, coordinate, origin, theta): + ido = coordinate[0] + keido = coordinate[1] + ido0 = origin[0] + keido0 = origin[1] + + # self.get_logger().info(f"theta: {theta}") + + a = 6378137 + f = 35/10439 + e1 = 734/8971 + e2 = 127/1547 + n = 35/20843 + a0 = 1 + a2 = 102/40495 + a4 = 1/378280 + a6 = 1/289634371 + a8 = 1/204422462123 + pi180 = 71/4068 + # %math.pi/180 + d_ido = ido - ido0 + d_keido = keido - keido0 + rd_ido = d_ido * pi180 + rd_keido = d_keido * pi180 + r_ido = ido * pi180 + r_keido = keido * pi180 + r_ido0 = ido0 * pi180 + W = math.sqrt(1-(e1**2)*(math.sin(r_ido)**2)) + N = a / W + t = math.tan(r_ido) + ai = e2*math.cos(r_ido) + + # %===Y===% + S = a*(a0*r_ido - a2*math.sin(2*r_ido)+a4*math.sin(4*r_ido) - + a6*math.sin(6*r_ido)+a8*math.sin(8*r_ido))/(1+n) + S0 = a*(a0*r_ido0-a2*math.sin(2*r_ido0)+a4*math.sin(4*r_ido0) - + a6*math.sin(6*r_ido0)+a8*math.sin(8*r_ido0))/(1+n) + m0 = S/S0 + B = S-S0 + y1 = (rd_keido**2)*N*math.sin(r_ido)*math.cos(r_ido)/2 + y2 = (rd_keido**4)*N*math.sin(r_ido) * \ + (math.cos(r_ido)**3)*(5-(t**2)+9*(ai**2)+4*(ai**4))/24 + y3 = (rd_keido**6)*N*math.sin(r_ido)*(math.cos(r_ido)**5) * \ + (61-58*(t**2)+(t**4)+270*(ai**2)-330*(ai**2)*(t**2))/720 + gps_y = self.Position_magnification * m0 * (B + y1 + y2 + y3) + + # %===X===% + x1 = rd_keido*N*math.cos(r_ido) + x2 = (rd_keido**3)*N*(math.cos(r_ido)**3)*(1-(t**2)+(ai**2))/6 + x3 = (rd_keido**5)*N*(math.cos(r_ido)**5) * \ + (5-18*(t**2)+(t**4)+14*(ai**2)-58*(ai**2)*(t**2))/120 + gps_x = self.Position_magnification * m0 * (x1 + x2 + x3) + + # point = (gps_x, gps_y)Not match + + degree_to_radian = math.pi / 180 + r_theta = theta * degree_to_radian + h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y + h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y + + point = (h_y, -h_x) + + return point + + def publish_fix(self, gps): + #lonlat = self.get_gps_quat(self.dev_name, self.country_id) + #if lonlat: + self.lonlat_msg.header = Header() + self.lonlat_msg.header.frame_id = "gps" + self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() + + self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if gps[ + 0] != 0 else NavSatStatus.STATUS_NO_FIX + #self.lonlat_msg.latitude = float(lonlat[1]) + #self.lonlat_msg.longitude = float(lonlat[2]) + #self.lonlat_msg.altitude = float(lonlat[3]) + self.lonlat_msg.latitude = gps[1] + self.lonlat_msg.longitude = gps[2] + self.lonlat_msg.altitude = gps[3] + + self.lonlat_pub.publish(self.lonlat_msg) + # self.get_logger().info(f"Published GPS data: {lonlat}") + #else: + # self.get_logger().error("!!!!-gps data error-!!!!") + + def publish_movingbase(self, heading): + if heading is not None and heading != 0.0: + robotheading = heading + 90.0 + if robotheading >= 360.0: + robotheading -= 360.0 + + if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!") + self.first_heading = robotheading + self.count = 1 + + relative_heading = robotheading - self.first_heading + if relative_heading < 0: + relative_heading += 360.0 + if relative_heading > 180.0: + relative_heading -= 360.0 + + movingbaseyaw = relative_heading * (math.pi / 180.0) + + roll, pitch = 0.0, 0.0 + yaw = movingbaseyaw + q = self.quaternion_from_euler(roll, pitch, yaw) + + self.movingbase_msg.header.stamp = self.get_clock().now().to_msg() + self.movingbase_msg.header.frame_id = "imu_link" + self.movingbase_msg.orientation.x = q[1] + self.movingbase_msg.orientation.y = q[2] + self.movingbase_msg.orientation.z = -q[3] # -z方向 + self.movingbase_msg.orientation.w = q[0] + self.movingbase_msg.orientation_covariance[0] = robotheading + + self.movingbase_pub.publish(self.movingbase_msg) + + else: + self.movingbase_msg.header.stamp = self.get_clock().now().to_msg() + self.movingbase_msg.header.frame_id = "imu_link" + self.movingbase_msg.orientation.x = 0.0 + self.movingbase_msg.orientation.y = 0.0 + self.movingbase_msg.orientation.z = 0.0 + self.movingbase_msg.orientation.w = 0.0 + + self.movingbase_pub.publish(self.movingbase_msg) + self.get_logger().error("!!!!-not movingbase data-!!!!") + + + def publish_odom(self, lat, lon, alt): + + #GPS_data = self.get_gps_quat(self.dev_name, self.country_id) + #gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading) + #if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0: + if self.gps_data_cache and self.gps_data_cache[1] != 0 and self.gps_data_cache[2] != 0: + GPS_data = self.gps_data_cache + self.satelite = GPS_data[4] + lonlat = [GPS_data[1], GPS_data[2]] + + if self.initial_coordinate is None: + self.initial_coordinate = [GPS_data[1], GPS_data[2]] + GPSxy = self.conversion(lonlat, self.initial_coordinate, self.theta) + GPSquat = self.heading_to_quat(GPS_data[5]) + + self.odom_msg.header.stamp = self.get_clock().now().to_msg() + self.odom_msg.header.frame_id = "odom" + self.odom_msg.child_frame_id = "base_footprint" + self.odom_msg.pose.pose.position.x = GPSxy[0] + self.odom_msg.pose.pose.position.y = GPSxy[1] + + self.odom_msg.pose.pose.orientation.x = GPSquat[1] + self.odom_msg.pose.pose.orientation.y = GPSquat[2] + self.odom_msg.pose.pose.orientation.z = -GPSquat[3] + self.odom_msg.pose.pose.orientation.w = GPSquat[0] + # Number of satellites + self.odom_msg.pose.covariance[0] = float(self.satelite) + self.odom_pub.publish(self.odom_msg) + else: + self.get_logger().error("!!!!-NOT RECIEVE-gps data error-!!!!") + +def main(args=None): + #rclpy.init(args=args) + #gpslonlat = GPSData() + #rclpy.spin(gpslonlat) + #gpslonlat.root.mainloop() + #gpslonlat.destroy_node() + #rclpy.shutdown() + rclpy.init(args=args) + gpslonlat = GPSData() + ros_thread = threading.Thread(target=rclpy.spin, args=(gpslonlat,)) + ros_thread.start() + gpslonlat.root.mainloop() # tkinter GUI表示 + gpslonlat.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() + diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py new file mode 100644 index 0000000..7e2973d --- /dev/null +++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py @@ -0,0 +1,391 @@ +#!/usr/bin/env python3 +import rclpy +import serial +import math +import tkinter as tk +from nav_msgs.msg import Odometry +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix, NavSatStatus +from std_msgs.msg import Header +import threading +import time +from my_msgs.srv import Avglatlon + +class GPSData(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + self.declare_parameter('port', '/dev/sensors/GNSS_UM982') + self.declare_parameter('baud', 115200) + self.declare_parameter('country_id', 0) + self.declare_parameter('Position_magnification', 1.675) + self.declare_parameter('heading', 180.0) + + self.dev_name = self.get_parameter('port').get_parameter_value().string_value + self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value + self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value + self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value + #self.theta = self.get_parameter('heading').get_parameter_value().double_value + + self.initial_coordinate = None + self.fix_data = None + self.count = 0 + + self.odom_pub = self.create_publisher(Odometry, "/odom/UM982", 10) + self.odom_msg = Odometry() + + self.lonlat_pub = self.create_publisher(NavSatFix, "/fix", 1) + self.lonlat_msg = NavSatFix() + + self.initialized = False # 平均初期座標が取得できたかどうか + self.timer = self.create_timer(1.0, self.publish_GPS_lonlat_quat) + + # service client + self.client = self.create_client(Avglatlon, 'send_avg_gps') + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available...") + + self.get_logger().info("Start get_lonlat quat node") + self.get_logger().info("-------------------------") + + # tkinter GUI setup + self.root = tk.Tk() + self.root.title("GPS Data Acquisition") + self.start_button = tk.Button(self.root, text="Start GPS Acquisition", command=self.start_gps_acquisition) + self.start_button.pack() + + self.gps_acquisition_thread = None + self.is_acquiring = False + + # service client + def send_request(self): + request = Avglatlon.Request() + request.avg_lat = self.initial_coordinate[0] # ← average lat + request.avg_lon = self.initial_coordinate[1] # ← average lon + request.theta = self.theta + + future = self.client.call_async(request) + future.add_done_callback(self.response_callback) + + def response_callback(self, future): + try: + response = future.result() + if response.success: + self.get_logger().info('サービス送信成功') + else: + self.get_logger().warn('サービスは受け取られましたが、処理は失敗しました') + except Exception as e: + self.get_logger().error(f'サービス呼び出し失敗: {e}') + + # gps data collect + def start_gps_acquisition(self): + if not self.is_acquiring: + self.is_acquiring = True + self.gps_acquisition_thread = threading.Thread(target=self.acquire_gps_data) + self.gps_acquisition_thread.start() + + def acquire_gps_data(self): + lat_sum = 0.0 + lon_sum = 0.0 + heading_sum = 0.0 + count = 0 + #serial_port = serial.Serial(self.dev_name, self.serial_baud) + #line = serial_port.readline() + + start_time = time.time() + while time.time() - start_time < 10: # 10 seconds + GPS_data = self.get_gps_quat(self.dev_name, self.country_id) + #gps_data = line.split(b",") + if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0: + lat_sum += GPS_data[1] + lon_sum += GPS_data[2] + heading_sum += float(GPS_data[5]) + count += 1 + time.sleep(0.1) # Slight delay to avoid overwhelming the GPS device + + if count > 0: + self.initial_coordinate = [lat_sum / count, lon_sum / count] + self.theta = (heading_sum / count) - 90 + self.initialized = True + self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}") + self.get_logger().info(f"Initial theta set to: {self.theta}") + self.send_request() + self.is_acquiring = False + + def get_gps_quat(self, dev_name, country_id): + # interface with sensor device(as a serial port) + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + # country info + if country_id == 0: # Japan + initial_letters = b"GNGGA" + elif country_id == 1: # USA + initial_letters = b"GPGGA" + else: # not certain + initial_letters = None + + initial_letters_outdoor = b"$GNHDT" + initial_letters_indoor = b"$GPHDT" + + while(1): + line = serial_port.readline() + #self.get_logger().info(f"line: {line}") + talker_ID_indoor = line.find(initial_letters_indoor) + talker_ID_outdoor = line.find(initial_letters_outdoor) + if talker_ID_indoor != -1: + #self.get_logger().info("GPHDT ok") + #line = line[(talker_ID_indoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + if talker_ID_outdoor != -1: + #self.get_logger().info("GNHDT ok") + #line = line[(talker_ID_outdoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + +# gps_data = ["$G?GGA", +# "UTC time", +# "Latitude (ddmm.mmmmm)", +# "latitude type (south/north)", +# "Longitude (ddmm.mmmmm)", +# "longitude type (east longitude/west longitude)", +# "Fixtype", +# "Number of satellites used for positioning", +# "HDOP", +# "Altitude", +# "M(meter)", +# "Elevation", +# "M(meter)", +# "", +# "checksum"] + + line = serial_port.readline() + talker_ID = line.find(initial_letters) + if talker_ID != -1: + line = line[(talker_ID-1):] + gps_data = line.split(b",") + Fixtype_data = int(gps_data[6]) + if Fixtype_data != 0: + satelitecount_data = int(gps_data[7])### + if Fixtype_data != 0: + latitude_data = float(gps_data[2]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[3] == b"S":#south + latitude_data *= -1 + longitude_data = float(gps_data[4]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[5] == b"W":#west + longitude_data *= -1 + altitude_data = float(gps_data[9]) + else : + #not fix data + Fixtype_data = 0 + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not fix data--!") + else : + #no GPS data + Fixtype_data = 0 + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not GPS data--!") + + serial_port.close() + + gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading) + return gnggadata + + def quaternion_from_euler(self, roll, pitch, yaw): + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + return q + + + def heading_to_quat(self ,real_heading): + + robotheading = real_heading - 90 + if robotheading >= 360: + robotheading -= 360 + + self.get_logger().info(f"real_heading: {real_heading}") + self.get_logger().info(f"robotheading: {robotheading}") + + if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!") + self.first_heading = robotheading + self.count = 1 + + relative_heading = robotheading - self.first_heading + if relative_heading < 0: + relative_heading += 360 + + if relative_heading > 180: + relative_heading -= 360 + + movingbaseyaw = relative_heading * (math.pi / 180) + + roll, pitch = 0.0, 0.0 + yaw = movingbaseyaw + + q = self.quaternion_from_euler(roll, pitch, yaw) + # self.get_logger().info(f"Quaternion: {q}") + + return q + + def conversion(self, coordinate, origin, theta): + ido = coordinate[0] + keido = coordinate[1] + ido0 = origin[0] + keido0 = origin[1] + + # self.get_logger().info(f"theta: {theta}") + + a = 6378137 + f = 35/10439 + e1 = 734/8971 + e2 = 127/1547 + n = 35/20843 + a0 = 1 + a2 = 102/40495 + a4 = 1/378280 + a6 = 1/289634371 + a8 = 1/204422462123 + pi180 = 71/4068 + # %math.pi/180 + d_ido = ido - ido0 + d_keido = keido - keido0 + rd_ido = d_ido * pi180 + rd_keido = d_keido * pi180 + r_ido = ido * pi180 + r_keido = keido * pi180 + r_ido0 = ido0 * pi180 + W = math.sqrt(1-(e1**2)*(math.sin(r_ido)**2)) + N = a / W + t = math.tan(r_ido) + ai = e2*math.cos(r_ido) + + # %===Y===% + S = a*(a0*r_ido - a2*math.sin(2*r_ido)+a4*math.sin(4*r_ido) - + a6*math.sin(6*r_ido)+a8*math.sin(8*r_ido))/(1+n) + S0 = a*(a0*r_ido0-a2*math.sin(2*r_ido0)+a4*math.sin(4*r_ido0) - + a6*math.sin(6*r_ido0)+a8*math.sin(8*r_ido0))/(1+n) + m0 = S/S0 + B = S-S0 + y1 = (rd_keido**2)*N*math.sin(r_ido)*math.cos(r_ido)/2 + y2 = (rd_keido**4)*N*math.sin(r_ido) * \ + (math.cos(r_ido)**3)*(5-(t**2)+9*(ai**2)+4*(ai**4))/24 + y3 = (rd_keido**6)*N*math.sin(r_ido)*(math.cos(r_ido)**5) * \ + (61-58*(t**2)+(t**4)+270*(ai**2)-330*(ai**2)*(t**2))/720 + gps_y = self.Position_magnification * m0 * (B + y1 + y2 + y3) + + # %===X===% + x1 = rd_keido*N*math.cos(r_ido) + x2 = (rd_keido**3)*N*(math.cos(r_ido)**3)*(1-(t**2)+(ai**2))/6 + x3 = (rd_keido**5)*N*(math.cos(r_ido)**5) * \ + (5-18*(t**2)+(t**4)+14*(ai**2)-58*(ai**2)*(t**2))/120 + gps_x = self.Position_magnification * m0 * (x1 + x2 + x3) + + # point = (gps_x, gps_y)Not match + + degree_to_radian = math.pi / 180 + r_theta = theta * degree_to_radian + h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y + h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y + #point = (-h_y, h_x) + point = (h_y, -h_x) + + return point + + def publish_GPS_lonlat_quat(self): + if not self.initialized: + # 初期化が完了していないので何もしない + return + + GPS_data = self.get_gps_quat(self.dev_name, self.country_id) + #gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading) + if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0: + self.satelite = GPS_data[4] + lonlat = [GPS_data[1], GPS_data[2]] + + # publish fix topic + #self.lonlat_msg.header = Header() + #self.lonlat_msg.frame_id = "gps" + #self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() + + #self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if lonlat[ + # 0] != 0 else NavSatStatus.STATUS_NO_FIX + #self.lonlat_msg.latitude = GPS_data[1] # ido + #self.lonlat_msg.longitude = GPS_data[2] # keido + #self.lonlat_msg.altitude = GPS_data[3] # koudo + + #self.lonlat_pub.publish(self.lonlat_msg) + # self.get_logger().info(f"Published GPS data: {lonlat}") + + if self.initial_coordinate is None: + self.initial_coordinate = [GPS_data[1], GPS_data[2]] + GPSxy = self.conversion(lonlat, self.initial_coordinate, self.theta) + GPSquat = self.heading_to_quat(GPS_data[5]) + + self.odom_msg.header.stamp = self.get_clock().now().to_msg() + self.odom_msg.header.frame_id = "odom" + self.odom_msg.child_frame_id = "base_footprint" + self.odom_msg.pose.pose.position.x = GPSxy[0] + self.odom_msg.pose.pose.position.y = GPSxy[1] + + self.odom_msg.pose.pose.orientation.x = GPSquat[1] + self.odom_msg.pose.pose.orientation.y = GPSquat[2] + self.odom_msg.pose.pose.orientation.z = -GPSquat[3] + self.odom_msg.pose.pose.orientation.w = GPSquat[0] + # Number of satellites + self.odom_msg.pose.covariance[0] = float(self.satelite) + self.odom_pub.publish(self.odom_msg) + else: + self.get_logger().error("!!!!-NOT RECIEVE-gps data error-!!!!") + + +def main(args=None): + #rclpy.init(args=args) + #gpslonlat = GPSData() + #rclpy.spin(gpslonlat) + #gpslonlat.root.mainloop() + #gpslonlat.destroy_node() + #rclpy.shutdown() + rclpy.init(args=args) + gpslonlat = GPSData() + + ros_thread = threading.Thread(target=rclpy.spin, args=(gpslonlat,)) + ros_thread.start() + + gpslonlat.root.mainloop() # tkinter GUI表示 + + gpslonlat.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/orange_bringup/orange_bringup/lonlat_to_odom.py b/orange_gnss/orange_gnss/lonlat_to_odom.py similarity index 98% rename from orange_bringup/orange_bringup/lonlat_to_odom.py rename to orange_gnss/orange_gnss/lonlat_to_odom.py index 63685aa..c604822 100755 --- a/orange_bringup/orange_bringup/lonlat_to_odom.py +++ b/orange_gnss/orange_gnss/lonlat_to_odom.py @@ -111,8 +111,7 @@ def conversion(self, coordinate, origin, theta): r_theta = theta * degree_to_radian h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y - point = (h_y, -h_x) - # point = (-h_y, h_x) + point = (-h_y, h_x) # point = (h_y, -h_x) return point diff --git a/orange_bringup/orange_bringup/movingbase_yaw_to_quat.py b/orange_gnss/orange_gnss/movingbase_yaw_to_quat.py similarity index 94% rename from orange_bringup/orange_bringup/movingbase_yaw_to_quat.py rename to orange_gnss/orange_gnss/movingbase_yaw_to_quat.py index adea144..b781fd7 100644 --- a/orange_bringup/orange_bringup/movingbase_yaw_to_quat.py +++ b/orange_gnss/orange_gnss/movingbase_yaw_to_quat.py @@ -32,7 +32,7 @@ def __init__(self): self.movingbase_data = None self.get_logger().info("Start movingbase_yaw_to_quat node") - self.get_logger().info("---------------------------------") + self.get_logger().info("-------------------------") # def movingbase_callback(self, data): # self.movingbase_data = data @@ -80,7 +80,7 @@ def checksum(self, ackPacket, payloadlength): return True else: self.get_logger().error("ACK Checksum Failure") - self.get_logger().error("!!! -movingbase receive error- !!!") + self.get_logger().error("!!!!-movingbase receive error-!!!!") return False def parse_heading(self, ackPacket): @@ -138,9 +138,10 @@ def movingbase_publish_msg(self): if heading >= 360: heading -= 360 - self.get_logger().info(f"robotheading: {heading}") + #self.get_logger().info(f"robotheading: {heading}") if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {heading} deg----------!!!") self.first_heading = heading self.count = 1 @@ -170,7 +171,7 @@ def movingbase_publish_msg(self): self.heading_pub.publish(self.movingbase_msg) self.movingbase_data = None else: - self.get_logger().error("!!! -movingbase data error- !!!") + self.get_logger().error("!!!!-movingbase data error-!!!!") def main(args=None): diff --git a/orange_gnss/package.xml b/orange_gnss/package.xml new file mode 100644 index 0000000..637ce96 --- /dev/null +++ b/orange_gnss/package.xml @@ -0,0 +1,23 @@ + + + + orange_gnss + 0.0.0 + For GNSS + Saito + Apache-2.0 + orange_description + rviz2 + rclpy + std_msgs + geometry_msgs + nav_msgs + python3-pymodbus + velodyne + urg_node + icm_20948 + joint_state_publisher + + ament_python + + diff --git a/orange_gnss/resource/orange_gnss b/orange_gnss/resource/orange_gnss new file mode 100644 index 0000000..e69de29 diff --git a/orange_gnss/setup.cfg b/orange_gnss/setup.cfg new file mode 100644 index 0000000..fa25766 --- /dev/null +++ b/orange_gnss/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/orange_gnss +[install] +install_scripts=$base/lib/orange_gnss diff --git a/orange_gnss/setup.py b/orange_gnss/setup.py new file mode 100644 index 0000000..a675aff --- /dev/null +++ b/orange_gnss/setup.py @@ -0,0 +1,42 @@ +import os +from glob import glob + +from setuptools import setup + +package_name = "orange_gnss" + +setup( + name=package_name, + version="0.0.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", + ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer=["Saito"], + maintainer_email=[ + "shun.saito.6t@stu.hosei.ac.jp", + ], + description="For GNSS", + license="Apache License, Version 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "GPSodom_correction = orange_gnss.GPSodom_correction:main", + "combination = orange_gnss.combination:main", + "ekf_myself = orange_gnss.ekf_myself:main", + "ekf_myself_noGPS = orange_gnss.ekf_myself_noGPS:main", + "fix_to_GPSodom = orange_gnss.fix_to_GPSodom:main", + "get_lonlat_ttyACM = orange_gnss.get_lonlat_ttyACM:main", + "get_lonlat_ttyUSB = orange_gnss.get_lonlat_ttyUSB:main", + "get_movingbase_quat_ttyUSB = orange_gnss.get_movingbase_quat_ttyUSB:main", + "gnss_odom_movingbase_fix_publisher_ttyUSB = orange_gnss.gnss_odom_movingbase_fix_publisher_ttyUSB:main", + "gnss_odom_publisher_ttyUSB = orange_gnss.gnss_odom_publisher_ttyUSB:main", + "lonlat_to_odom = orange_gnss.lonlat_to_odom:main", + "movingbase_yaw_to_quat = orange_gnss.movingbase_yaw_to_quat:main" + ], + }, +) From aed0b292acede30fafc2597c8e06e4bacf93d37f Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 11:08:51 +0000 Subject: [PATCH 3/4] Update launch files. --- .../launch/data_processing.launch.xml | 13 +++---- orange_bringup/launch/orange_robot.launch.xml | 16 ++------- orange_bringup/launch/with_ros2bag.launch.xml | 34 +++---------------- 3 files changed, 13 insertions(+), 50 deletions(-) diff --git a/orange_bringup/launch/data_processing.launch.xml b/orange_bringup/launch/data_processing.launch.xml index 31a414f..0a28aa3 100644 --- a/orange_bringup/launch/data_processing.launch.xml +++ b/orange_bringup/launch/data_processing.launch.xml @@ -16,17 +16,14 @@ --> - - - - + - + - + @@ -44,11 +41,11 @@ - + diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index 32c4dea..1a77510 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -62,29 +62,19 @@ - + - - - - - + - + - - - - - - - - + - + - + - - - - - - - - - - - - - - -