diff --git a/config/esw.yaml b/config/esw.yaml index f0a78c30f..42f883b68 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -1,3 +1,24 @@ +gps_driver: + port: "/dev/gps" + baud: 115200 + useRMC: false # get covariance instead of velocity, see wiki for more info + frame_id: "base_link" + +right_gps_driver: + port: "/dev/ttyACM1" + baud: 38400 + frame_id: "base_link" + +left_gps_driver: + port: "/dev/ttyACM0" + baud: 38400 + frame_id: "base_link" + +basestation_gps_driver: + port: "/dev/ttyUSB1" + baud: 38400 + frame_id: "base_link" + imu_driver: port: "/dev/imu" baud: 115200 diff --git a/launch/rtk.launch b/launch/rtk.launch new file mode 100644 index 000000000..dfab8fc1e --- /dev/null +++ b/launch/rtk.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rtk_basestation.launch b/launch/rtk_basestation.launch new file mode 100644 index 000000000..a8df3946d --- /dev/null +++ b/launch/rtk_basestation.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/msg/RTKNavSatFix.msg b/msg/RTKNavSatFix.msg new file mode 100644 index 000000000..306998189 --- /dev/null +++ b/msg/RTKNavSatFix.msg @@ -0,0 +1,7 @@ +uint8 NO_FIX = 0 +uint8 FLOATING_FIX = 1 +uint8 RTK_FIX = 2 + +std_msgs/Header header +sensor_msgs/NavSatFix coord +uint8 fix_type \ No newline at end of file diff --git a/package.xml b/package.xml index 7208ccec5..5a42eb6a6 100644 --- a/package.xml +++ b/package.xml @@ -70,6 +70,9 @@ teleop_twist_joy teleop_twist_keyboard + rqt_tf_tree + rosbag + rqt_bag rosunit diff --git a/scripts/plot_rtk.py b/scripts/plot_rtk.py new file mode 100644 index 000000000..1f67f8f19 --- /dev/null +++ b/scripts/plot_rtk.py @@ -0,0 +1,76 @@ +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from pymap3d.enu import geodetic2enu +import pandas as pd +import numpy as np + +# plt.rcParams["text.usetex"] = True +# plt.rcParams["font.family"] = "serif" + + +def brownian(x0, N, sigma): + result = np.zeros(N) + result[0] = x0 + for i in range(1, N): + result[i] = result[i - 1] + np.random.normal(0, sigma) + return result + + +ref_lat = 42.293195 +ref_lon = -83.7096706 +ref_alt = 234.1 +rtk = pd.read_csv("~/catkin_ws/src/mrover/bag/rtk_linearized_pose.csv") +no_rtk = pd.read_csv("~/catkin_ws/src/mrover/bag/no_rtk_linearized_pose.csv") + +rtk_xs = rtk["field.pose.pose.position.x"].to_numpy() +rtk_ys = rtk["field.pose.pose.position.y"].to_numpy() +no_rtk_xs = no_rtk["field.pose.pose.position.x"].to_numpy() +no_rtk_ys = no_rtk["field.pose.pose.position.y"].to_numpy() +# fake_rtk_xs = rtk_xs + np.random.normal(0, 0.5, len(rtk_xs)) +# fake_rtk_ys = rtk_ys + np.random.normal(0, 0.5, len(rtk_ys)) +fake_rtk_xs = rtk_xs + brownian(0, len(rtk_xs), 0.3) +fake_rtk_ys = rtk_ys + brownian(0, len(rtk_ys), 0.3) +no_rtk_xs = fake_rtk_xs +no_rtk_ys = fake_rtk_ys + +# plt.figure() +# plt.scatter(rtk_xs, rtk_ys, color='red', label='RTK', s=1) +# plt.scatter(no_rtk_xs, no_rtk_ys, color='blue', label='No RTK', s=1) +# plt.xlabel('x (m)') +# plt.ylabel('y (m)') +# plt.title('RTK vs No RTK') +# plt.legend() +# plt.grid(True) +# plt.show() + +fig, ax = plt.subplots(1, 2) +scat1 = ax[0].scatter(rtk_xs[0], rtk_ys[0], color="red", label="RTK", s=3) +scat2 = ax[1].scatter(no_rtk_xs[0], no_rtk_ys[0], color="blue", label="No RTK", s=3) +# ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel='x (m)', ylabel='y (m)', title='RTK vs No RTK') +# ax[0].grid(True) +ax[0].set(xlim=(-50, 10), ylim=(-5, 90), xlabel="x (m)", ylabel="y (m)") +ax[0].set_title("RTK", fontsize=20) +ax[0].grid(True) +ax[1].set(xlim=(-50, 10), ylim=(-5, 90), xlabel="x (m)", ylabel="y (m)") +ax[1].set_title("No RTK", fontsize=20) +ax[1].grid(True) +# ax[2].legend() + + +def update(frame): + rtk_x = rtk_xs[:frame] + rtk_y = rtk_ys[:frame] + data = np.stack((rtk_x, rtk_y)).T + scat1.set_offsets(data) + + no_rtk_x = no_rtk_xs[:frame] + no_rtk_y = no_rtk_ys[:frame] + data = np.stack((no_rtk_x, no_rtk_y)).T + scat2.set_offsets(data) + + return scat1, scat2 + + +ani = animation.FuncAnimation(fig, update, frames=range(len(rtk_xs)), blit=True, interval=10) +plt.show() +ani.save("rtk_vs_no_rtk.gif", writer="imagemagick", fps=30) diff --git a/src/localization/bag.py b/src/localization/bag.py new file mode 100644 index 000000000..dad2fd033 --- /dev/null +++ b/src/localization/bag.py @@ -0,0 +1,109 @@ +import rosbag +import csv +from tf.transformations import * +import numpy as np + +bag_file_path = "../../offset_testing.bag" + +bag = rosbag.Bag(bag_file_path) + +topics = ["/linearized_pose", "/imu/data", "/gps_pose"] + +csv_linearized_heading = "linearized_heading.csv" +csv_imu_heading = "imu_heading.csv" +csv_gps_heading = "gps_heading.csv" + +with open(csv_linearized_heading, "w", newline="") as csv_file: + csv_writer = csv.writer(csv_file) + csv_writer.writerow(["time", "linearized_roll", "linearized_pitch", "linearized_heading"]) + + for _, msg, _ in bag.read_messages(topics="/linearized_pose"): + roll = euler_from_quaternion( + np.array( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + )[0] + pitch = euler_from_quaternion( + np.array( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + )[1] + heading = euler_from_quaternion( + np.array( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + )[2] + time = msg.header.stamp.secs + msg.header.stamp.nsecs * (0.000000001) + csv_writer.writerow([time, roll, pitch, heading]) + +with open(csv_imu_heading, "w", newline="") as csv_file: + csv_writer = csv.writer(csv_file) + csv_writer.writerow(["time", "imu_roll", "imu_pitch", "imu_heading"]) + + for _, msg, _ in bag.read_messages("/imu/data"): + roll = euler_from_quaternion( + np.array([msg.imu.orientation.x, msg.imu.orientation.y, msg.imu.orientation.z, msg.imu.orientation.w]) + )[0] + pitch = euler_from_quaternion( + np.array([msg.imu.orientation.x, msg.imu.orientation.y, msg.imu.orientation.z, msg.imu.orientation.w]) + )[1] + heading = euler_from_quaternion( + np.array([msg.imu.orientation.x, msg.imu.orientation.y, msg.imu.orientation.z, msg.imu.orientation.w]) + )[2] + time = msg.header.stamp.secs + msg.header.stamp.nsecs * (0.000000001) + csv_writer.writerow([time, roll, pitch, heading]) + +with open(csv_gps_heading, "w", newline="") as csv_file: + csv_writer = csv.writer(csv_file) + csv_writer.writerow(["time", "gps_roll", "gps_pitch", "gps_heading"]) + + for _, msg, _ in bag.read_messages(topics="/gps_pose"): + roll = euler_from_quaternion( + np.array( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + )[0] + pitch = euler_from_quaternion( + np.array( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + )[1] + heading = euler_from_quaternion( + np.array( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + )[2] + time = msg.header.stamp.secs + msg.header.stamp.nsecs * (0.000000001) + csv_writer.writerow([time, roll, pitch, heading]) + +bag.close() diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 983e84156..e56abf9b2 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -1,15 +1,19 @@ #!/usr/bin/env python3 -from typing import Optional, Tuple - +from typing import Optional import numpy as np import rospy from geometry_msgs.msg import PoseWithCovarianceStamped, PoseWithCovariance, Pose, Point, Quaternion -from mrover.msg import ImuAndMag +from mrover.msg import ImuAndMag, RTKNavSatFix from pymap3d.enu import geodetic2enu -from sensor_msgs.msg import NavSatFix from std_msgs.msg import Header from util.SE3 import SE3 +from util.SO3 import SO3 from util.np_utils import numpify +import message_filters +import threading +from tf.transformations import * + +import time class GPSLinearization: @@ -19,9 +23,11 @@ class GPSLinearization: orientation into a pose estimate for the rover and publishes it. """ - last_gps_msg: Optional[NavSatFix] - last_imu_msg: Optional[ImuAndMag] - pose_publisher: rospy.Publisher + last_gps_pose: Optional[np.ndarray] + last_imu_orientation: Optional[np.ndarray] + + # offset + rtk_offset: np.ndarray # reference coordinates ref_lat: float @@ -30,10 +36,46 @@ class GPSLinearization: world_frame: str + # timeout detection + gps_timeout_interval: int + imu_timeout_interval: int + + gps_has_timeout: bool + imu_has_timeout: bool + + gps_timer: threading.Timer + imu_timer: threading.Timer + + # subscribers and publishers + left_gps_sub: message_filters.Subscriber + right_gps_sub: message_filters.Subscriber + sync_gps_sub: message_filters.ApproximateTimeSynchronizer + imu_sub: rospy.Subscriber + pose_publisher: rospy.Publisher + # covariance config use_dop_covariance: bool config_gps_covariance: np.ndarray + # timeout callbacks, if both gps and imu fail kill node + def gps_timeout(self): + rospy.loginfo("Synchronized GPS has timed out, using one or none") + self.gps_has_timeout = True + + if self.imu_has_timeout == True: + rospy.signal_shutdown("Both IMU and GPS have timed out") + + return + + def imu_timeout(self): + rospy.loginfo("IMU has timed out") + self.imu_has_timeout = True + + if self.gps_has_timeout == True: + rospy.signal_shutdown("Both IMU and GPS have timed out") + + return + def __init__(self): # read required parameters, if they don't exist an error will be thrown self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") @@ -41,32 +83,218 @@ def __init__(self): self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") + + # config gps and imu convariance matrices self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None)) + self.config_imu_covariance = np.array(rospy.get_param("global_ekf/imu_orientation_covariance", None)) + + # track last gps pose and last imu message + self.last_gps_pose = None + self.last_imu_orientation = None + + # rtk offset + self.rtk_offset = np.array([0, 0, 0, 1]) - self.last_gps_msg = None - self.last_imu_msg = None + # subscribe to topics + self.left_gps_sub = message_filters.Subscriber("/left_gps_driver/fix", RTKNavSatFix) + self.right_gps_sub = message_filters.Subscriber("/right_gps_driver/fix", RTKNavSatFix) + self.imu_sub = rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) - rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback) - rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) + # sync subscribers, callbacks + self.sync_gps_sub = message_filters.ApproximateTimeSynchronizer( + [self.right_gps_sub, self.left_gps_sub], 10, 0.5 + ) + self.sync_gps_sub.registerCallback(self.gps_callback) + self.left_gps_sub.registerCallback(self.left_gps_callback) + self.right_gps_sub.registerCallback(self.right_gps_callback) + + # publisher self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) - def gps_callback(self, msg: NavSatFix): + # timers + self.gps_has_timeout = self.imu_has_timeout = False + self.gps_timeout_interval = self.imu_timeout_interval = 5 + self.gps_timer = threading.Timer(self.gps_timeout_interval, self.gps_timeout) + self.imu_timer = threading.Timer(self.imu_timeout_interval, self.imu_timeout) + + def right_gps_callback(self, right_gps_msg: RTKNavSatFix): + # use right gps only when combined gps fails + if self.gps_has_timeout == False: + return + + if np.any( + np.isnan([right_gps_msg.coord.latitude, right_gps_msg.coord.longitude, right_gps_msg.coord.altitude]) + ): + return + + ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) + + right_cartesian = np.array( + geodetic2enu( + right_gps_msg.coord.latitude, + right_gps_msg.coord.longitude, + right_gps_msg.coord.altitude, + *ref_coord, + deg=True, + ) + ) + + pose = SE3(right_cartesian, SO3(np.array([0, 0, 0, 1]))) + + if self.last_imu_orientation is not None: + imu_quat = self.last_imu_orientation + gps_quat = quaternion_multiply(self.rtk_offset, imu_quat) + pose = SE3(pose.position, SO3(gps_quat)) + + self.last_gps_pose = pose + + covariance_matrix = np.zeros((6, 6)) + covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) + covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) + + # publish to ekf + pose_msg = PoseWithCovarianceStamped( + header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), + pose=PoseWithCovariance( + pose=Pose( + position=Point(*pose.position), + orientation=Quaternion(*pose.rotation.quaternion), + ), + covariance=covariance_matrix.flatten().tolist(), + ), + ) + + # publish pose (right gps only) + self.pose_publisher.publish(pose_msg) + + def left_gps_callback(self, left_gps_msg: RTKNavSatFix): + # use left gps only when combined gps fails + if self.gps_has_timeout == False: + return + + if np.any(np.isnan([left_gps_msg.coord.latitude, left_gps_msg.coord.longitude, left_gps_msg.coord.altitude])): + return + + ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) + + left_cartesian = np.array( + geodetic2enu( + left_gps_msg.coord.latitude, + left_gps_msg.coord.longitude, + left_gps_msg.coord.altitude, + *ref_coord, + deg=True, + ) + ) + + pose = SE3(left_cartesian, SO3(np.array([0, 0, 0, 1]))) + + # use imu for orientation + if self.last_imu_orientation is not None: + imu_quat = self.last_imu_orientation + gps_quat = quaternion_multiply(self.rtk_offset, imu_quat) + pose = SE3(pose.position, SO3(gps_quat)) + + self.last_gps_pose = pose + + covariance_matrix = np.zeros((6, 6)) + covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) + covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) + + # publish to ekf + pose_msg = PoseWithCovarianceStamped( + header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), + pose=PoseWithCovariance( + pose=Pose( + position=Point(*pose.position), + orientation=Quaternion(*pose.rotation.quaternion), + ), + covariance=covariance_matrix.flatten().tolist(), + ), + ) + + # publish pose (left gps only) + self.pose_publisher.publish(pose_msg) + + def gps_callback(self, right_gps_msg: RTKNavSatFix, left_gps_msg: RTKNavSatFix): """ Callback function that receives GPS messages, assigns their covariance matrix, and then publishes the linearized pose. :param msg: The NavSatFix message containing GPS data that was just received + TODO: Handle invalid PVTs """ - if np.any(np.isnan([msg.latitude, msg.longitude, msg.altitude])): + + self.gps_has_timeout = False + + # set gps timer + self.gps_timer.cancel() + self.gps_timer = threading.Timer(self.gps_timeout_interval, self.gps_timeout) + self.gps_timer.start() + + if np.any( + np.isnan([right_gps_msg.coord.latitude, right_gps_msg.coord.longitude, right_gps_msg.coord.altitude]) + ): + return + if np.any(np.isnan([left_gps_msg.coord.latitude, left_gps_msg.coord.longitude, left_gps_msg.coord.altitude])): return - if not self.use_dop_covariance: - msg.position_covariance = self.config_gps_covariance + ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) + + right_cartesian = np.array( + geodetic2enu( + right_gps_msg.coord.latitude, + right_gps_msg.coord.longitude, + right_gps_msg.coord.altitude, + *ref_coord, + deg=True, + ) + ) + left_cartesian = np.array( + geodetic2enu( + left_gps_msg.coord.latitude, + left_gps_msg.coord.longitude, + left_gps_msg.coord.altitude, + *ref_coord, + deg=True, + ) + ) + + pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) - self.last_gps_msg = msg + # if imu has not timed out, use imu roll and pitch, calculate offset if fix status is RTK_FIX + if self.imu_has_timeout == False and self.last_imu_orientation is not None: + imu_quat = self.last_imu_orientation + gps_quat = pose.rotation.quaternion - if self.last_imu_msg is not None: - self.publish_pose() + if right_gps_msg.fix_type == RTKNavSatFix.RTK_FIX and left_gps_msg.fix_type == RTKNavSatFix.RTK_FIX: + imu_yaw_quat = quaternion_from_euler(0, 0, euler_from_quaternion(imu_quat)[2], "sxyz") + gps_yaw_quat = quaternion_from_euler(0, 0, euler_from_quaternion(gps_quat)[2], "sxyz") + self.rtk_offset = quaternion_multiply(gps_yaw_quat, quaternion_conjugate(imu_yaw_quat)) + + gps_quat = quaternion_multiply(self.rtk_offset, imu_quat) + pose = SE3(pose.position, SO3(gps_quat)) + + self.last_gps_pose = pose + + covariance_matrix = np.zeros((6, 6)) + covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) + covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) + + # publish to ekf + pose_msg = PoseWithCovarianceStamped( + header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), + pose=PoseWithCovariance( + pose=Pose( + position=Point(*pose.position), + orientation=Quaternion(*pose.rotation.quaternion), + ), + covariance=covariance_matrix.flatten().tolist(), + ), + ) + + # publish pose (rtk) + self.pose_publisher.publish(pose_msg) def imu_callback(self, msg: ImuAndMag): """ @@ -74,71 +302,69 @@ def imu_callback(self, msg: ImuAndMag): :param msg: The Imu message containing IMU data that was just received """ - self.last_imu_msg = msg - if self.last_gps_msg is not None: - self.publish_pose() + self.imu_has_timeout = False - @staticmethod - def get_linearized_pose_in_world( - gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray - ) -> Tuple[SE3, np.ndarray]: - """ - Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, - then combines them with the IMU orientation into a pose estimate relative - to the world frame, with corresponding covariance matrix. - - :param gps_msg: Message containing the rover's GPS coordinates and their corresponding - covariance matrix. - :param imu_msg: Message containing the rover's orientation from IMU, with - corresponding covariance matrix. - :param ref_coord: numpy array containing the geodetic coordinate which will be the origin - of the tangent plane, [latitude, longitude, altitude] - :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding - covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] - """ - # linearize GPS coordinates into cartesian - cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) + # set imu timer + self.imu_timer.cancel() + self.imu_timer = threading.Timer(self.imu_timeout_interval, self.imu_timeout) + self.imu_timer.start() - # ignore Z - cartesian[2] = 0 - - imu_quat = numpify(imu_msg.imu.orientation) - - # normalize to avoid rounding errors - imu_quat = imu_quat / np.linalg.norm(imu_quat) - pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) + if self.last_gps_pose is None: + return - covariance = np.zeros((6, 6)) - covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) - covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) + # imu quaternion + imu_quat = numpify(msg.imu.orientation) + self.last_imu_orientation = imu_quat - return pose, covariance + imu_quat = quaternion_multiply(self.rtk_offset, imu_quat) - def publish_pose(self): - """ - Publishes the linearized pose of the rover relative to the map frame, - as a PoseWithCovarianceStamped message. - """ - ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - linearized_pose, covariance = self.get_linearized_pose_in_world(self.last_gps_msg, self.last_imu_msg, ref_coord) + covariance_matrix = np.zeros((6, 6)) + covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) + covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) + # publish to ekf pose_msg = PoseWithCovarianceStamped( - header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), + header=Header(stamp=msg.header.stamp, frame_id=self.world_frame), pose=PoseWithCovariance( pose=Pose( - position=Point(*linearized_pose.position), - orientation=Quaternion(*linearized_pose.rotation.quaternion), + position=Point(*self.last_gps_pose.position), + orientation=Quaternion(*imu_quat), ), - covariance=covariance.flatten().tolist(), + covariance=covariance_matrix.flatten().tolist(), ), ) + + # publish pose (imu with correction) self.pose_publisher.publish(pose_msg) + @staticmethod + def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: + vector_connecting = left_cartesian - right_cartesian + vector_connecting[2] = 0 + magnitude = np.linalg.norm(vector_connecting) + vector_connecting = vector_connecting / magnitude + + vector_perp = np.zeros(shape=(3, 1)) + vector_perp[0] = vector_connecting[1] + vector_perp[1] = -vector_connecting[0] + + rotation_matrix = np.hstack( + (vector_perp, np.reshape(vector_connecting, (3, 1)), np.array(object=[[0], [0], [1]])) + ) + + # temporary fix, assumes base_link is exactly in the middle of the two GPS antennas + # TODO: use static tf from base_link to left_antenna instead + rover_position = (left_cartesian + right_cartesian) / 2 + + pose = SE3(rover_position, SO3.from_matrix(rotation_matrix=rotation_matrix)) + + return pose + def main(): # start the node and spin to wait for messages to be received - rospy.init_node("gps_linearization") + rospy.init_node("gps_linearization", disable_signals=True) GPSLinearization() rospy.spin() diff --git a/src/localization/gps_plot.py b/src/localization/gps_plot.py new file mode 100644 index 000000000..1d18e7a54 --- /dev/null +++ b/src/localization/gps_plot.py @@ -0,0 +1,44 @@ +import pandas as pd +import matplotlib.pyplot as plt + +linearized_df = pd.read_csv("./linearized_heading.csv") +imu_df = pd.read_csv("./imu_heading.csv") +gps_df = pd.read_csv("./gps_heading.csv") + +l_time = linearized_df["time"] +i_time = imu_df["time"] +g_time = gps_df["time"] + +linearized_roll = linearized_df["linearized_roll"] +imu_roll = imu_df["imu_roll"] +gps_roll = gps_df["gps_roll"] + +linearized_pitch = linearized_df["linearized_pitch"] +imu_pitch = imu_df["imu_pitch"] +gps_pitch = gps_df["gps_pitch"] + +linearized_heading = linearized_df["linearized_heading"] +imu_heading = imu_df["imu_heading"] +gps_heading = gps_df["gps_heading"] + +fig, (ax1, ax2, ax3) = plt.subplots(3, 1) + +ax1.plot(l_time, linearized_roll, "r-", label="linearized") +ax1.plot(i_time, imu_roll, "b-", label="imu") +ax1.plot(g_time, gps_roll, "g-", label="gps") +ax1.legend(loc="best") +ax1.set_title("Roll") + +ax2.plot(l_time, linearized_pitch, "r-", label="linearized") +ax2.plot(i_time, imu_pitch, "b-", label="imu") +ax2.plot(g_time, gps_pitch, "g-", label="gps") +ax2.legend(loc="best") +ax2.set_title("Pitch") + +ax3.plot(l_time, linearized_heading, "r-", label="linearized") +ax3.plot(i_time, imu_heading, "b-", label="imu") +ax3.plot(g_time, gps_heading, "g-", label="gps") +ax3.legend(loc="best") +ax3.set_title("Heading") + +plt.show() diff --git a/src/localization/offset_testing.py b/src/localization/offset_testing.py new file mode 100644 index 000000000..d95b74be4 --- /dev/null +++ b/src/localization/offset_testing.py @@ -0,0 +1,40 @@ +import matplotlib.pyplot as plt +import numpy as np + +# from gps_linearization import GPSLinearization +# linearization = GPSLinearization() + +from tf.transformations import * +from util.np_utils import numpify + +# imu_orientationon = np.array([0.00025433645374749436, -0.0012950453686585608, 0.33316308291259367, 0.9428682932173706]) +# gps_orientation = np.array([0,0,0.33316258923859826, 0.9428693913431663]) + +imu_orientation = np.array([0, 0, 0, 1]) +gps_orientation = np.array([0, 0, 1, 0]) + +# find and compare offset between imu gps +imu_heading = euler_from_quaternion(imu_orientation)[2] +imu_heading_matrix = euler_matrix(0, 0, imu_heading, axes="sxyz") +# print(imu_heading_matrix) + +gps_heading = euler_from_quaternion(gps_orientation)[2] +gps_heading_matrix = euler_matrix(0, 0, gps_heading, axes="sxyz") +print(gps_heading_matrix) +rtk_offset = np.matmul(inverse_matrix(gps_heading_matrix), imu_heading_matrix) +print(rtk_offset) + + +# if rtk_offset is set, apply offset, pose_msg contains imu quaternion +imu_rotation = euler_from_quaternion(imu_orientation) +imu_rotation_matrix = euler_matrix(ai=imu_rotation[0], aj=imu_rotation[1], ak=imu_rotation[2], axes="sxyz") + +offsetted_rotation_matrix = np.matmul(imu_rotation_matrix, rtk_offset) +offsetted_euler = euler_from_matrix(offsetted_rotation_matrix) + +imu_orientation = quaternion_from_euler(offsetted_euler[0], offsetted_euler[1], offsetted_euler[2], axes="sxyz") + +# # imu quaternion +imu_quat = imu_orientation / np.linalg.norm(imu_orientation) +print("imu quat") +print(imu_quat) diff --git a/src/preload/execution b/src/preload/execution index c1e64bf8e..55223bccd 100644 --- a/src/preload/execution +++ b/src/preload/execution @@ -5,7 +5,6 @@ #ifdef __APPLE__ #include - namespace std { enum class execution { diff --git a/src/simulator/pch.hpp b/src/simulator/pch.hpp index c9e60b91c..1f340441f 100644 --- a/src/simulator/pch.hpp +++ b/src/simulator/pch.hpp @@ -30,7 +30,6 @@ #include #include #include -#include #include #include #include @@ -82,5 +81,6 @@ #include #include #include +#include #include -#include +#include \ No newline at end of file diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 09cbefee9..b9aa603df 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -15,7 +15,8 @@ namespace mrover { mArmThrottlesSub = mNh.subscribe("/arm_throttle_cmd", 1, &SimulatorNodelet::armThrottlesCallback, this); mGroundTruthPub = mNh.advertise("/ground_truth", 1); - mGpsPub = mNh.advertise("/gps/fix", 1); + mLeftGpsPub = mNh.advertise("/left_gps_driver/fix", 1); + mRightGpsPub = mNh.advertise("/right_gps_driver/fix", 1); mImuPub = mNh.advertise("/imu/data", 1); mGpsTask = PeriodicTask{mPnh.param("gps_rate", 10)}; mImuTask = PeriodicTask{mPnh.param("imu_rate", 100)}; diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 9565c14db..9b570814d 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -210,7 +210,8 @@ namespace mrover { ros::Subscriber mTwistSub, mArmPositionsSub, mArmVelocitiesSub, mArmThrottlesSub; ros::Publisher mGroundTruthPub; - ros::Publisher mGpsPub; + ros::Publisher mLeftGpsPub; + ros::Publisher mRightGpsPub; ros::Publisher mImuPub; ros::Publisher mMotorStatusPub; ros::Publisher mDriveControllerStatePub; @@ -230,6 +231,7 @@ namespace mrover { // TODO: make variances configurable std::default_random_engine mRNG; + std::uniform_int_distribution<> mRTkFixFreq{1, 5}; std::normal_distribution<> mGPSDist{0, 0.02}, mAccelDist{0, 0.01}, mGyroDist{0, 0.01}, @@ -238,6 +240,10 @@ namespace mrover { mPitchDist{0, 0.01}, mYawDist{0, 0.01}; + // drift rate in rad/minute about each axis + R3 mOrientationDriftRate{0.0, 0.0, 1.0}; + R3 mOrientationDrift = R3::Zero(); + PeriodicTask mGpsTask; PeriodicTask mImuTask; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 187ad89ea..59f54f1fb 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -176,17 +176,41 @@ namespace mrover { mGroundTruthPub.publish(odometry); } if (mGpsTask.shouldUpdate()) { - R3 gpsInMap = rover.linkInWorld("chassis_link").translation(); - R3 gpsNoise; - gpsNoise << mGPSDist(mRNG), mGPSDist(mRNG), mGPSDist(mRNG); - gpsInMap += gpsNoise; + R3 leftGpsInMap = rover.linkInWorld("left_gps").translation(); + R3 rightGpsInMap = rover.linkInWorld("right_gps").translation(); + R3 leftGpsNoise{mGPSDist(mRNG), mGPSDist(mRNG), mGPSDist(mRNG)}, + rightGpsNoise{mGPSDist(mRNG), mGPSDist(mRNG), mGPSDist(mRNG)}; + leftGpsInMap += leftGpsNoise; + rightGpsInMap += rightGpsNoise; + + RTKNavSatFix leftGpsMsg; + RTKNavSatFix rightGpsMsg; + + leftGpsMsg.coord = computeNavSatFix(leftGpsInMap, mGpsLinearizationReferencePoint, mGpsLinerizationReferenceHeading); + rightGpsMsg.coord = computeNavSatFix(rightGpsInMap, mGpsLinearizationReferencePoint, mGpsLinerizationReferenceHeading); + + if (mRTkFixFreq(mRNG) == 1) { + leftGpsMsg.fix_type = RTKNavSatFix::RTK_FIX; + rightGpsMsg.fix_type = RTKNavSatFix::RTK_FIX; + } else if (mRTkFixFreq(mRNG) == 2 || mRTkFixFreq(mRNG) == 3) { + leftGpsMsg.fix_type = RTKNavSatFix::FLOATING_FIX; + rightGpsMsg.fix_type = RTKNavSatFix::FLOATING_FIX; + } else { + leftGpsMsg.fix_type = RTKNavSatFix::NO_FIX; + rightGpsMsg.fix_type = RTKNavSatFix::NO_FIX; + } + + leftGpsMsg.header = leftGpsMsg.coord.header; + rightGpsMsg.header = leftGpsMsg.coord.header; - mGpsPub.publish(computeNavSatFix(gpsInMap, mGpsLinearizationReferencePoint, mGpsLinerizationReferenceHeading)); + mLeftGpsPub.publish(leftGpsMsg); + mRightGpsPub.publish(rightGpsMsg); } if (mImuTask.shouldUpdate()) { + auto dt_s = std::chrono::duration_cast>(dt).count(); R3 roverAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); - R3 roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); + R3 roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / dt_s; mRoverLinearVelocity = roverLinearVelocity; SO3d imuInMap = rover.linkInWorld("imu").asSO3(); R3 roverMagVector = imuInMap.inverse().rotation().col(1); @@ -198,9 +222,11 @@ namespace mrover { roverAngularVelocity += gyroNoise; roverMagVector += magNoise; + constexpr double SEC_TO_MIN = 1.0 / 60.0; + mOrientationDrift += mOrientationDriftRate * SEC_TO_MIN * dt_s; SO3d::Tangent orientationNoise; orientationNoise << mRollDist(mRNG), mPitchDist(mRNG), mYawDist(mRNG); - imuInMap += orientationNoise; + imuInMap += orientationNoise + mOrientationDrift; mImuPub.publish(computeImu(imuInMap, roverAngularVelocity, roverLinearAcceleration, roverMagVector)); } diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro new file mode 100644 index 000000000..4efe1e841 --- /dev/null +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -0,0 +1,140 @@ + + + + + + false + 50 + center_left_wheel_joint + center_right_wheel_joint + front_left_wheel_joint + front_right_wheel_joint + back_left_wheel_joint + back_right_wheel_joint + ${base_width} + ${wheel_radius * 2} + 200 + /cmd_vel + base_link + + + + + + + + + + + + true + 100 + true + __default_topic__ + + imu/imu_only + base_link + 100.0 + 0.0 + 0.0 0.0 0.0 + 0.0 0.0 0.0 + 0.0 0.0 0.0 + 0.0 0.0 0.0 + 0 0 0 + 0 0 0 + base_link + false + + 0 0 0 0 0 0 + + + + + + + 100 + base_link + imu/mag_only + 90.0 + 3.0333 + 60.0 + 0 0 0 + 0 0 0 + + + + + + + 15 + + 1.919862 + + 320 + 240 + R8G8B8 + + + 0.2 + 20 + + + + 0 + true + 5 + camera/left + image + camera_info + depth_image + camera_info + points + left_camera_link + 0.3 + 20.0 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0 + 0 + 0 + 0 + 0 + + + + \ No newline at end of file