Dual antenna RTK

CMakeLists.txt

@@ -65,6 +65,7 @@ find_package(ament_cmake_python REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(rtcm_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -226,6 +227,14 @@ target_link_libraries(simulator
 )
 target_include_directories(simulator SYSTEM PRIVATE ${BULLET_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS})
 
+# Localization
+
+mrover_add_node(rover_gps_driver localization/rover_gps_driver.cpp)
+mrover_add_node(heading_filter localization/heading_filter.cpp)
+ament_target_dependencies(rover_gps_driver rclcpp sensor_msgs rtcm_msgs)
+ament_target_dependencies(heading_filter rclcpp rcutils sensor_msgs builtin_interfaces message_filters)
+target_link_libraries(heading_filter lie)
+
 # Perception
 
 find_package(ZED QUIET)
@@ -314,9 +323,9 @@ endif ()
 install(PROGRAMS
         navigation/nav.py
         navigation/stuck_detector/stuck_detector.py
-        localization/rover_gps_driver.py
         localization/gps_linearization.py
         localization/basestation_gps_driver.py
+        localization/rover_gps_driver.py
         superstructure/superstructure.py
         scripts/debug_course_publisher.py
 		scripts/visualizer.py

ansible/roles/esw/files/rules/99-gps.rules

@@ -1 +1,2 @@
-SUBSYSTEM=="tty", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", SYMLINK+="gps"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", SYMLINK+="unicore_gps"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", SYMLINK+="ublox_gps"

config/localization.yaml

@@ -6,8 +6,8 @@
 
 basestation_gps_driver:
   ros__parameters:
-    port: /dev/gps
-    baudrate: 38400
+    port: /dev/ttyACM0
+    baud: 38400
 
 gps_linearization:
   ros__parameters:
@@ -18,3 +18,18 @@ gps_linearization:
     ref_lat: 42.293195
     ref_lon: -83.7096706
     ref_alt: 0.0
+
+rover_gps_driver:
+  ros__parameters:
+    port_unicore: /dev/unicore_gps
+    baud_unicore: 115200
+
+    port_ublox: /dev/ublox_gps
+    baud_ublox: 38400
+
+    frame_id: "base_link"
+
+heading_filter:
+  ros__parameters:
+    world_frame: "map"
+    rover_frame: "base_link"

config/navigation.yaml

@@ -48,8 +48,8 @@ navigation:
       post_radius: 0.7
 
     # TODO(quintin): Avoid storing these in multiple config files
-    ref_lat: 38.4225202
-    ref_lon: -110.7844653
+    ref_lat: 42.293195
+    ref_lon: -83.7096706
     ref_alt: 0.0
     world_frame: "map"
     rover_frame: "base_link"

gps_heading_test_11_7/metadata.yaml

@@ -0,0 +1,38 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 113137741590
+  starting_time:
+    nanoseconds_since_epoch: 1731027597084158407
+  message_count: 328
+  topics_with_message_count:
+    - topic_metadata:
+        name: /gps_fix_status
+        type: mrover/msg/RTKStatus
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 114
+    - topic_metadata:
+        name: /heading/fix
+        type: mrover/msg/RTKHeading
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 107
+    - topic_metadata:
+        name: /heading_fix_status
+        type: mrover/msg/RTKStatus
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 107
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - gps_heading_test_11_7_0.db3
+  files:
+    - path: gps_heading_test_11_7_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1731027597084158407
+      duration:
+        nanoseconds: 113137741590
+      message_count: 328

gps_heading_test_11_7/plots/heading.csv

@@ -0,0 +1,103 @@
+sec,nanosec,frame_id,heading
+1731027604,85061081,base_link,73.45649719238281
+1731027605,82084285,base_link,73.856201171875
+1731027606,84023647,base_link,71.16000366210938
+1731027607,82712144,base_link,73.13020324707031
+1731027608,84047645,base_link,72.3104019165039
+1731027609,85573731,base_link,70.939697265625
+1731027610,87011684,base_link,70.25060272216797
+1731027611,83140368,base_link,68.75499725341797
+1731027612,84828323,base_link,71.03170013427734
+1731027613,87211330,base_link,72.5166015625
+1731027614,82653974,base_link,73.560302734375
+1731027615,85049279,base_link,75.59400177001953
+1731027616,82464578,base_link,77.11260223388672
+1731027617,82944837,base_link,77.85549926757812
+1731027618,82880069,base_link,77.714599609375
+1731027619,82934766,base_link,77.51409912109375
+1731027620,82858304,base_link,78.53569793701172
+1731027621,83003547,base_link,78.91020202636719
+1731027622,83160682,base_link,79.88189697265625
+1731027623,83671521,base_link,81.01850128173828
+1731027624,82689860,base_link,81.50129699707031
+1731027625,83455125,base_link,81.96179962158203
+1731027626,82936605,base_link,81.91500091552734
+1731027627,82943248,base_link,82.59960174560547
+1731027628,82963260,base_link,83.15789794921875
+1731027629,82622040,base_link,86.73159790039062
+1731027630,82718403,base_link,86.2030029296875
+1731027631,83054613,base_link,85.90139770507812
+1731027632,82679246,base_link,86.4031982421875
+1731027633,83858873,base_link,86.66709899902344
+1731027634,82658168,base_link,86.77539825439453
+1731027635,83031157,base_link,86.85659790039062
+1731027636,82165454,base_link,86.50599670410156
+1731027637,82924936,base_link,86.59500122070312
+1731027638,82371201,base_link,86.43419647216797
+1731027639,83481076,base_link,86.49320220947266
+1731027640,82504616,base_link,86.05159759521484
+1731027641,83055028,base_link,86.00669860839844
+1731027642,82495010,base_link,86.1416015625
+1731027643,83570808,base_link,86.26950073242188
+1731027644,82709777,base_link,86.69509887695312
+1731027645,83689532,base_link,86.11699676513672
+1731027646,82416148,base_link,86.89029693603516
+1731027647,82924772,base_link,87.08580017089844
+1731027648,84295764,base_link,86.69580078125
+1731027649,83010652,base_link,87.15119934082031
+1731027650,82540613,base_link,86.83209991455078
+1731027651,82999949,base_link,87.13500213623047
+1731027652,82506952,base_link,86.74600219726562
+1731027653,83372604,base_link,86.93789672851562
+1731027654,82458225,base_link,86.63749694824219
+1731027655,82586495,base_link,86.75330352783203
+1731027656,82657836,base_link,85.0602035522461
+1731027657,82927895,base_link,77.90769958496094
+1731027658,82635457,base_link,60.66339874267578
+1731027659,82476728,base_link,43.50619888305664
+1731027660,82518176,base_link,28.01490020751953
+1731027661,82482986,base_link,7.320899963378906
+1731027662,82575368,base_link,359.23699951171875
+1731027663,83492779,base_link,358.3830871582031
+1731027664,82528445,base_link,357.8280029296875
+1731027665,83383126,base_link,357.8638916015625
+1731027666,82559011,base_link,357.6134033203125
+1731027667,83499259,base_link,357.5331115722656
+1731027668,83134578,base_link,355.3941955566406
+1731027669,83487084,base_link,344.5635986328125
+1731027670,83605443,base_link,324.78961181640625
+1731027671,83423740,base_link,300.63909912109375
+1731027672,82576028,base_link,282.9892883300781
+1731027673,84491749,base_link,268.40069580078125
+1731027674,82620697,base_link,267.0712890625
+1731027675,83255011,base_link,266.2868957519531
+1731027676,82529793,base_link,267.0177917480469
+1731027677,82655397,base_link,266.36468505859375
+1731027678,83319547,base_link,266.55731201171875
+1731027679,83617073,base_link,266.6708984375
+1731027680,82680919,base_link,266.2319030761719
+1731027681,82868677,base_link,263.0567932128906
+1731027682,82614113,base_link,253.32749938964844
+1731027683,84398389,base_link,238.64500427246094
+1731027684,82403979,base_link,226.42449951171875
+1731027685,82641401,base_link,210.3656005859375
+1731027686,82505176,base_link,195.07139587402344
+1731027687,82824407,base_link,182.0970001220703
+1731027688,83330605,base_link,175.8072052001953
+1731027689,82606973,base_link,175.6719970703125
+1731027690,82408003,base_link,175.5166015625
+1731027691,82734099,base_link,175.6009063720703
+1731027692,82370163,base_link,175.47300720214844
+1731027693,83751406,base_link,172.57110595703125
+1731027694,82851220,base_link,152.59120178222656
+1731027695,82751594,base_link,133.09080505371094
+1731027696,83205960,base_link,105.65799713134766
+1731027697,83338033,base_link,90.21320343017578
+1731027698,82870570,base_link,87.75589752197266
+1731027699,84732138,base_link,87.66010284423828
+1731027700,82405903,base_link,87.16320037841797
+1731027701,82866597,base_link,87.04609680175781
+1731027702,82139476,base_link,87.86229705810547
+1731027703,84456053,base_link,88.19920349121094
+1731027704,83476846,base_link,84.28019714355469
+1731027710,86701479,base_link,77.95339965820312

gps_heading_test_11_7/plots/heading_fix_status.csv

@@ -0,0 +1,103 @@
+sec,nanosec,frame_id,fix_status
+1731027604,85061081,base_link,1
+1731027605,82084285,base_link,1
+1731027606,84023647,base_link,1
+1731027607,82712144,base_link,1
+1731027608,84047645,base_link,1
+1731027609,85573731,base_link,1
+1731027610,87011684,base_link,1
+1731027611,83140368,base_link,1
+1731027612,84828323,base_link,1
+1731027613,87211330,base_link,1
+1731027614,82653974,base_link,1
+1731027615,85049279,base_link,1
+1731027616,82464578,base_link,1
+1731027617,82944837,base_link,1
+1731027618,82880069,base_link,1
+1731027619,82934766,base_link,1
+1731027620,82858304,base_link,1
+1731027621,83003547,base_link,1
+1731027622,83160682,base_link,1
+1731027623,83671521,base_link,1
+1731027624,82689860,base_link,1
+1731027625,83455125,base_link,1
+1731027626,82936605,base_link,1
+1731027627,82943248,base_link,1
+1731027628,82963260,base_link,1
+1731027629,82622040,base_link,2
+1731027630,82718403,base_link,2
+1731027631,83054613,base_link,2
+1731027632,82679246,base_link,2
+1731027633,83858873,base_link,2
+1731027634,82658168,base_link,2
+1731027635,83031157,base_link,2
+1731027636,82165454,base_link,2
+1731027637,82924936,base_link,2
+1731027638,82371201,base_link,2
+1731027639,83481076,base_link,2
+1731027640,82504616,base_link,2
+1731027641,83055028,base_link,2
+1731027642,82495010,base_link,2
+1731027643,83570808,base_link,2
+1731027644,82709777,base_link,2
+1731027645,83689532,base_link,2
+1731027646,82416148,base_link,2
+1731027647,82924772,base_link,2
+1731027648,84295764,base_link,2
+1731027649,83010652,base_link,2
+1731027650,82540613,base_link,2
+1731027651,82999949,base_link,2
+1731027652,82506952,base_link,2
+1731027653,83372604,base_link,2
+1731027654,82458225,base_link,2
+1731027655,82586495,base_link,2
+1731027656,82657836,base_link,2
+1731027657,82927895,base_link,2
+1731027658,82635457,base_link,2
+1731027659,82476728,base_link,2
+1731027660,82518176,base_link,2
+1731027661,82482986,base_link,2
+1731027662,82575368,base_link,2
+1731027663,83492779,base_link,2
+1731027664,82528445,base_link,2
+1731027665,83383126,base_link,2
+1731027666,82559011,base_link,2
+1731027667,83499259,base_link,2
+1731027668,83134578,base_link,2
+1731027669,83487084,base_link,2
+1731027670,83605443,base_link,2
+1731027671,83423740,base_link,2
+1731027672,82576028,base_link,2
+1731027673,84491749,base_link,2
+1731027674,82620697,base_link,2
+1731027675,83255011,base_link,2
+1731027676,82529793,base_link,2
+1731027677,82655397,base_link,2
+1731027678,83319547,base_link,2
+1731027679,83617073,base_link,2
+1731027680,82680919,base_link,2
+1731027681,82868677,base_link,2
+1731027682,82614113,base_link,2
+1731027683,84398389,base_link,2
+1731027684,82403979,base_link,1
+1731027685,82641401,base_link,1
+1731027686,82505176,base_link,2
+1731027687,82824407,base_link,2
+1731027688,83330605,base_link,2
+1731027689,82606973,base_link,2
+1731027690,82408003,base_link,2
+1731027691,82734099,base_link,2
+1731027692,82370163,base_link,2
+1731027693,83751406,base_link,2
+1731027694,82851220,base_link,2
+1731027695,82751594,base_link,2
+1731027696,83205960,base_link,2
+1731027697,83338033,base_link,2
+1731027698,82870570,base_link,2
+1731027699,84732138,base_link,2
+1731027700,82405903,base_link,2
+1731027701,82866597,base_link,2
+1731027702,82139476,base_link,2
+1731027703,84456053,base_link,2
+1731027704,83476846,base_link,2
+1731027710,86701479,base_link,1

gps_heading_test_11_7/plots/plot_heading.py

@@ -0,0 +1,18 @@
+import pandas as pd
+import matplotlib.pyplot as plt
+
+heading = pd.read_csv("heading.csv")
+heading_status = pd.read_csv("heading_fix_status.csv")
+
+heading_deg = heading["heading"]
+sec = heading["sec"]
+status = heading_status["fix_status"]
+
+
+plt.figure(figsize=(8, 6))
+plt.scatter(sec,heading_deg,c=status,cmap=plt.cm.Accent,s=10)
+plt.xlabel("time (s)")
+plt.ylabel("heading (deg)")
+plt.title("Moving heading test (fixed, No RTK, untethered)")
+
+plt.savefig("heading.png")

gps_heading_test_11_8/metadata.yaml

@@ -0,0 +1,38 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 127000395958
+  starting_time:
+    nanoseconds_since_epoch: 1731077517861000594
+  message_count: 384
+  topics_with_message_count:
+    - topic_metadata:
+        name: /gps_fix_status
+        type: mrover/msg/FixStatus
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 128
+    - topic_metadata:
+        name: /heading/fix
+        type: mrover/msg/Heading
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 128
+    - topic_metadata:
+        name: /heading_fix_status
+        type: mrover/msg/FixStatus
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 128
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - rosbag2_2024_11_08-09_51_57_0.db3
+  files:
+    - path: rosbag2_2024_11_08-09_51_57_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 1731077517861000594
+      duration:
+        nanoseconds: 127000395958
+      message_count: 384