Rtk filtering offset

ansible/jetson.yml

@@ -4,7 +4,7 @@
     ros_distro: noetic
     ubuntu_release: focal
   roles:
-    - jetson_build
-    - dev
+    # - jetson_build
+    # - dev
     - jetson_networks
-    - jetson_services
+    # - jetson_services

ansible/roles/jetson_networks/tasks/main.yml

@@ -4,7 +4,7 @@
     conn_name: MRover
     state: present
     type: ethernet
-    ifname: eth0
+    ifname: enp1s0
     autoconnect: yes
     ip4: 10.1.0.10/8
     gw4: 10.1.0.2

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

launch/rtk.launch

@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+
+  <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
+  <rosparam command="load" file="$(find mrover)/config/localization.yaml" />
+  <rosparam command="load" file="$(find mrover)/config/ekf.yaml" />
+
+  <!-- <node pkg="mrover" type="basestation_gps_driver.py" name="basestation_gps_driver" output="screen"/> -->
+
+
+  <group ns="right_gps_driver">
+    <node pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
+  </group>
+
+  <group ns="left_gps_driver">
+    <node pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
+  </group>
+
+  <node pkg="mrover" type="gps_linearization.py" name="gps_linearization" output="screen"/>
+
+
+
+</launch>

launch/rtk_basestation.launch

@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+
+    <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
+    <node pkg="mrover" type="basestation_gps_driver.py" name="basestation_gps_driver"
+        output="screen" />
+
+
+    <!-- <group ns="right_gps_driver">
+        <node pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen" />
+    </group> -->
+
+    <!-- <group ns="left_gps_driver">
+    <node pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
+  </group>
+ -->
+
+</launch>

msg/RTKStatus.msg

@@ -0,0 +1,6 @@
+uint8 NO_RTK = 0
+uint8 FLOATING_RTK = 1
+uint8 RTK_FIX = 2
+
+std_msgs/Header header
+uint8 RTK_FIX_TYPE

package.xml

@@ -70,6 +70,9 @@
   <!-- Utility -->
   <exec_depend>teleop_twist_joy</exec_depend>
   <exec_depend>teleop_twist_keyboard</exec_depend>
+  <depend>rqt_tf_tree</depend>
+  <depend>rosbag</depend>
+  <depend>rqt_bag</depend>
 
   <!-- Test -->
   <test_depend>rosunit</test_depend>

scripts/plot_rtk.py

@@ -0,0 +1,72 @@
+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)

src/localization/bag.py

@@ -0,0 +1,49 @@
+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()

src/localization/gps_driver.py

@@ -18,7 +18,6 @@
 from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol, UBXMessage
 from std_msgs.msg import Header
 from sensor_msgs.msg import NavSatFix
-
 # from rtcm_msgs.msg import Message
 # from mrover.msg import rtkStatus
 import datetime
@@ -78,7 +77,7 @@ def parse_rover_gps_data(self, rover_gps_data) -> None:
             parsed_latitude = msg.lat
             parsed_longitude = msg.lon
             parsed_altitude = msg.hMSL
-            time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour=msg.hour, second=msg.second)
+            time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour= msg.hour, second=msg.second)
             time = rospy.Time(secs=time.timestamp() + (msg.nano / 1e6))
             if not self.valid_offset:
                 self.time_offset = rospy.Time.now() - time