diff --git a/launch/localization.launch.py b/launch/localization.launch.py index 1f5aca5b..8d792b94 100644 --- a/launch/localization.launch.py +++ b/launch/localization.launch.py @@ -12,7 +12,6 @@ def generate_launch_description(): - rover_gps_driver_node = Node( package="mrover", executable="rover_gps_driver", @@ -34,11 +33,4 @@ def generate_launch_description(): parameters=[os.path.join(get_package_share_directory("mrover"), "config", "zed.yaml")], ) - - return LaunchDescription( - [ - rover_gps_driver_node, - gps_linearization_node, - zed_node - ] - ) + return LaunchDescription([rover_gps_driver_node, gps_linearization_node, zed_node]) diff --git a/launch/localization_basestation.launch.py b/launch/localization_basestation.launch.py index e219ac8d..88ae6f77 100644 --- a/launch/localization_basestation.launch.py +++ b/launch/localization_basestation.launch.py @@ -12,7 +12,6 @@ def generate_launch_description(): - basestation_gps_driver_node = Node( package="mrover", executable="basestation_gps_driver.py", @@ -20,7 +19,6 @@ def generate_launch_description(): parameters=[os.path.join(get_package_share_directory("mrover"), "config", "localization.yaml")], ) - return LaunchDescription( [ basestation_gps_driver_node, diff --git a/localization/basestation_gps_driver.py b/localization/basestation_gps_driver.py index 1d6e6332..bd0681f8 100755 --- a/localization/basestation_gps_driver.py +++ b/localization/basestation_gps_driver.py @@ -65,17 +65,17 @@ def spin(self) -> None: for i in range(msg.numSvs): gnssId = getattr(msg, f"gnssId_{i+1:02d}") cno = getattr(msg, f"cno_{i+1:02d}") - if (gnssId == 0): + if gnssId == 0: self.get_logger().info(f"GPS signal strength: {cno}") - elif (gnssId == 1): + elif gnssId == 1: self.get_logger().info(f"SBAS signal strength: {cno}") - elif (gnssId == 2): + elif gnssId == 2: self.get_logger().info(f"Galileo signal strength: {cno}") - elif (gnssId == 3): + elif gnssId == 3: self.get_logger().info(f"BeiDou signal strength: {cno}") - elif (gnssId == 5): + elif gnssId == 5: self.get_logger().info(f"QZSS signal strength: {cno}") - elif (gnssId == 6): + elif gnssId == 6: self.get_logger().info(f"GLONASS signal strength: {cno}") rclpy.spin_once(self, timeout_sec=0) diff --git a/localization/gps_linearization.py b/localization/gps_linearization.py index 29598a4f..37e1f059 100755 --- a/localization/gps_linearization.py +++ b/localization/gps_linearization.py @@ -26,6 +26,7 @@ import message_filters from lie.conversions import to_tf_tree, SE3 + class GPSLinearization(Node): def __init__(self) -> None: @@ -54,25 +55,23 @@ def __init__(self) -> None: self.sync = message_filters.ApproximateTimeSynchronizer([self.gps_sub, self.orientation_sub], 10, 1) self.sync.registerCallback(self.synced_gps_imu_callback) - def synced_gps_imu_callback(self, gps_msg: NavSatFix, imu_msg: Imu): if np.isnan([gps_msg.latitude, gps_msg.longitude, gps_msg.altitude]).any(): self.get_logger().warn("Received NaN GPS data, ignoring") return - quaternion = np.array([imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w]) + quaternion = np.array( + [imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w] + ) quaternion = quaternion / np.linalg.norm(quaternion) - x, y, z = geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, self.ref_lat, self.ref_lon, self.ref_alt, deg=True) + x, y, z = geodetic2enu( + gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, self.ref_lat, self.ref_lon, self.ref_alt, deg=True + ) pose = SE3(position=np.array([x, y, z]), quaternion=quaternion) - to_tf_tree( - tf_broadcaster=self.tf_broadcaster, - se3=pose, - child_frame="base_link", - parent_frame=self.world_frame - ) + to_tf_tree(tf_broadcaster=self.tf_broadcaster, se3=pose, child_frame="base_link", parent_frame=self.world_frame) self.get_logger().info(f"Published to TF Tree: Position({x}, {y}, {z}), Orientation({imu_msg.orientation})")